diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 7cfb5a52a82dd..f52eeb4e3ab2c 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -2,17 +2,14 @@ checkersReport constParameterReference -constVariableReference funcArgNamesDifferent functionConst functionStatic -knownConditionTrueFalse missingInclude missingIncludeSystem noConstructor passedByValue redundantInitialization -shadowVariable // cspell: ignore uninit uninitMemberVar unknownMacro diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 47ddd77bdf68e..9f530e0dea9e3 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -5,7 +5,7 @@ common/autoware_grid_map_utils/** maxime.clement@tier4.jp common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai -common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp +common/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -22,7 +22,7 @@ common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp +common/object_recognition_utils/** 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/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp @@ -34,7 +34,7 @@ common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosu common/tier4_api_utils/** isamu.takagi@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp -common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp +common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp yamato.ando@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai @@ -104,47 +104,45 @@ map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakam 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_bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@tier4.jp +perception/autoware_cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/autoware_detected_object_feature_remover/** tomoya.kimura@tier4.jp +perception/autoware_detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@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/autoware_elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp +perception/autoware_euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp +perception/autoware_lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@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_multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@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_occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_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/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@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/autoware_detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp -perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp -perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/lidar_apollo_instance_segmentation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com yoshi.ri@tier4.jp -perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com yoshi.ri@tier4.jp -perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp -perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp -perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/autoware_radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@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 -perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp -perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/autoware_tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp +perception/autoware_tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +perception/autoware_tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp +perception/autoware_traffic_light_classifier/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_traffic_light_fine_detector/** shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/autoware_traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp yukihiro.saito@tier4.jp +perception/autoware_traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/autoware_traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/autoware_traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -164,7 +162,7 @@ planning/autoware_rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tie planning/autoware_scenario_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 planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp -planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp +planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -201,23 +199,26 @@ 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/autoware_image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp +sensing/autoware_image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp +sensing/autoware_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/autoware_radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +sensing/autoware_radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +sensing/autoware_radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +sensing/autoware_radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@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/** taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/tier4_pcl_extensions/** david.wong@tier4.jp kenzo.lobos@tier4.jp ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp +simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail.com simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp +system/autoware_component_monitor/** memin@leodrive.ai +system/autoware_processing_time_checker/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp system/bluetooth_monitor/** fumihito.ito@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 151890e2783d9..e0748f1113db6 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -38,7 +38,7 @@ files: - source: .github/workflows/clang-tidy-differential.yaml pre-commands: | - sd 'container: ros:(\w+)' 'container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda' {source} + sd 'container: ros:(\w+)' 'container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe-cuda' {source} - source: .github/workflows/check-build-depends.yaml - source: .github/workflows/clang-tidy-pr-comments.yaml - source: .github/workflows/clang-tidy-pr-comments-manually.yaml diff --git a/.github/workflows/build-and-test-daily-arm64.yaml b/.github/workflows/build-and-test-daily-arm64.yaml index 65196f5b77a35..0d71e13a9ff58 100644 --- a/.github/workflows/build-and-test-daily-arm64.yaml +++ b/.github/workflows/build-and-test-daily-arm64.yaml @@ -19,7 +19,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml index 816bcd412bb4c..0b21ba56640a7 100644 --- a/.github/workflows/build-and-test-daily.yaml +++ b/.github/workflows/build-and-test-daily.yaml @@ -19,7 +19,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/build-and-test-differential-arm64.yaml b/.github/workflows/build-and-test-differential-arm64.yaml index 3be2792e0929d..73aead76a25f4 100644 --- a/.github/workflows/build-and-test-differential-arm64.yaml +++ b/.github/workflows/build-and-test-differential-arm64.yaml @@ -29,7 +29,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe build-depends-repos: build_depends.repos steps: - name: Set PR fetch depth diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 287769d0708ac..8d2158f3b911c 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -33,7 +33,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe build-depends-repos: build_depends.repos steps: - name: Set PR fetch depth @@ -130,7 +130,7 @@ jobs: clang-tidy-differential: needs: build-and-test-differential runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda + container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe-cuda steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 72f8679bff6ef..85dc6c2fb89bf 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -23,7 +23,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt + container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe build-depends-repos: build_depends.repos steps: - name: Check out repository diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index 81b192a1c2095..98646d8161da2 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -36,3 +36,5 @@ jobs: config.max_model_tokens: 64000 pr_code_suggestions.max_context_tokens: 12000 pr_code_suggestions.commitable_code_suggestions: true + pr_reviewer.enable_review_labels_effort: false + pr_reviewer.enable_review_labels_security: false diff --git a/build_depends.repos b/build_depends.repos index 0522862dd0eb7..67e06dba923f0 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -8,15 +8,15 @@ repositories: core/autoware_cmake: type: git url: https://github.com/autowarefoundation/autoware_cmake.git - version: main + version: 1.0.0 core/autoware_utils: type: git url: https://github.com/autowarefoundation/autoware_utils.git - version: main + version: 1.0.0 core/autoware_lanelet2_extension: type: git url: https://github.com/autowarefoundation/autoware_lanelet2_extension.git - version: main + version: 0.5.0 core/autoware.core: type: git url: https://github.com/autowarefoundation/autoware.core.git @@ -24,15 +24,15 @@ repositories: core/autoware_msgs: type: git url: https://github.com/autowarefoundation/autoware_msgs.git - version: main + version: 1.1.0 core/autoware_adapi_msgs: type: git url: https://github.com/autowarefoundation/autoware_adapi_msgs.git - version: main + version: 1.3.0 core/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: main + version: 1.1.0 # universe universe/external/tier4_autoware_msgs: type: git diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp index ab5abfe3c3d83..c3db6a2fec2ef 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp @@ -61,6 +61,10 @@ private Q_SLOTS: rviz_common::properties::IntProperty * property_height_; rviz_common::properties::IntProperty * property_right_; rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::ColorProperty * property_bg_color_; + rviz_common::properties::FloatProperty * property_bg_alpha_; + rviz_common::properties::ColorProperty * property_text_color_; + std::unique_ptr remaining_distance_time_topic_property_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp index 843b75e352648..d3f246f47129d 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp @@ -36,7 +36,8 @@ class RemainingDistanceTimeDisplay { public: RemainingDistanceTimeDisplay(); - void drawRemainingDistanceTimeDisplay(QPainter & painter, const QRectF & backgroundRect); + void drawRemainingDistanceTimeDisplay( + QPainter & painter, const QRectF & backgroundRect, const QColor & text_color); void updateRemainingDistanceTimeData( const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg); @@ -44,8 +45,6 @@ class RemainingDistanceTimeDisplay double remaining_distance_; // Internal variable to store remaining distance double remaining_time_; // Internal variable to store remaining time - QColor gray = QColor(194, 194, 194); - QImage icon_dist_; QImage icon_dist_scaled_; QImage icon_time_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp index bafae1727b2f1..2e76a2ae38f31 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp @@ -39,6 +39,12 @@ MissionDetailsDisplay::MissionDetailsDisplay() "Right", 10, "Margin from the right border", this, SLOT(update_size())); property_top_ = new rviz_common::properties::IntProperty( "Top", 10, "Margin from the top border", this, SLOT(update_size())); + property_bg_alpha_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.3, "Background Alpha", this, SLOT(update_size())); + property_bg_color_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Background Color", this, SLOT(update_size())); + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(194, 194, 194), "Text Color", this, SLOT(update_size())); // Initialize the component displays remaining_distance_time_display_ = std::make_unique(); @@ -154,7 +160,8 @@ void MissionDetailsDisplay::draw_widget(QImage & hud) draw_rounded_rect(painter, backgroundRect); if (remaining_distance_time_display_) { - remaining_distance_time_display_->drawRemainingDistanceTimeDisplay(painter, backgroundRect); + remaining_distance_time_display_->drawRemainingDistanceTimeDisplay( + painter, backgroundRect, property_text_color_->getColor()); } painter.end(); @@ -164,8 +171,10 @@ void MissionDetailsDisplay::draw_rounded_rect(QPainter & painter, const QRectF & { painter.setRenderHint(QPainter::Antialiasing, true); QColor colorFromHSV; - colorFromHSV.setHsv(0, 0, 29); - colorFromHSV.setAlphaF(0.60); + colorFromHSV.setHsv( + property_bg_color_->getColor().hue(), property_bg_color_->getColor().saturation(), + property_bg_color_->getColor().value()); + colorFromHSV.setAlphaF(property_bg_alpha_->getFloat()); painter.setBrush(colorFromHSV); diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp index 86395ef1918bc..13f3fdddfe76b 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp @@ -60,7 +60,7 @@ void RemainingDistanceTimeDisplay::updateRemainingDistanceTimeData( } void RemainingDistanceTimeDisplay::drawRemainingDistanceTimeDisplay( - QPainter & painter, const QRectF & backgroundRect) + QPainter & painter, const QRectF & backgroundRect, const QColor & text_color) { const QFont font("Quicksand", 15, QFont::Bold); painter.setFont(font); @@ -107,7 +107,7 @@ void RemainingDistanceTimeDisplay::drawRemainingDistanceTimeDisplay( QRectF rect_text_unit( rect.topRight().x() - unit_width, rect.topRight().y(), unit_width, rect.height()); - painter.setPen(gray); + painter.setPen(text_color); painter.drawText(rect_text_unit, Qt::AlignLeft | Qt::AlignVCenter, str_unit); // place the value text diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp index cb7e49685d0b3..057cce9718c56 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -36,7 +36,9 @@ class GearDisplay { public: GearDisplay(); - void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); + void drawGearIndicator( + QPainter & painter, const QRectF & backgroundRect, const QColor & color, + const QColor & bg_color); void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); private: diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index 5f5c9cee2da43..f8d822473c415 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -77,6 +77,12 @@ private Q_SLOTS: rviz_common::properties::IntProperty * property_top_; rviz_common::properties::ColorProperty * property_signal_color_; rviz_common::properties::FloatProperty * property_handle_angle_scale_; + rviz_common::properties::ColorProperty * property_background_color_; + rviz_common::properties::FloatProperty * property_background_alpha_; + rviz_common::properties::ColorProperty * property_primary_color_; + rviz_common::properties::ColorProperty * property_light_limit_color_; + rviz_common::properties::ColorProperty * property_dark_limit_color_; + std::unique_ptr steering_topic_property_; std::unique_ptr gear_topic_property_; std::unique_ptr speed_topic_property_; @@ -103,8 +109,7 @@ private Q_SLOTS: turn_signals_sub_; rclcpp::Subscription::SharedPtr hazard_lights_sub_; - rclcpp::Subscription::SharedPtr - traffic_sub_; + rclcpp::Subscription::SharedPtr traffic_sub_; rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; @@ -118,7 +123,7 @@ private Q_SLOTS: const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg); void drawWidget(QImage & hud); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp index 2ae8e9a3fe0b9..82d6cccd17ad8 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -36,12 +36,11 @@ class SpeedDisplay { public: SpeedDisplay(); - void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); + void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect, const QColor & color); void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_speed_; // Internal variable to store current speed - QColor gray = QColor(194, 194, 194); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp index fcdb293fe8c72..c88984a4c339f 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -37,14 +37,16 @@ class SpeedLimitDisplay { public: SpeedLimitDisplay(); - void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); + void drawSpeedLimitIndicator( + QPainter & painter, const QRectF & backgroundRect, const QColor & color, + const QColor & light_color, const QColor & dark_color, const QColor & bg_color, + const float bg_alpha); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_limit; // Internal variable to store current gear float current_speed_; // Internal variable to store current speed - QColor gray = QColor(194, 194, 194); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp index fd15f542021f1..01545bcea6f4e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -25,7 +25,6 @@ #include #include -#include #include #include @@ -40,8 +39,8 @@ class TrafficDisplay TrafficDisplay(); void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg); - autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_; + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficLightGroup current_traffic_; private: QImage traffic_light_image_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp index 956172bef6ed6..03729d122e614 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -54,7 +54,8 @@ void GearDisplay::updateGearData(const autoware_vehicle_msgs::msg::GearReport::C current_gear_ = msg->report; // Assuming msg->report contains the gear information } -void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroundRect) +void GearDisplay::drawGearIndicator( + QPainter & painter, const QRectF & backgroundRect, const QColor & color, const QColor & bg_color) { // we deal with the different gears here std::string gearString; @@ -90,9 +91,9 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun double gearX = backgroundRect.left() + 54; double gearY = backgroundRect.height() / 2 - gearBoxSize / 2; QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize); - painter.setBrush(gray); + painter.setBrush(color); painter.drawRoundedRect(gearRect, 10, 10); - painter.setPen(Qt::black); + painter.setPen(bg_color); painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString)); } diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index 1fd33a95fb34d..270d1837f2dba 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -52,6 +52,20 @@ SignalDisplay::SignalDisplay() property_handle_angle_scale_ = new rviz_common::properties::FloatProperty( "Handle Angle Scale", 17.0, "Scale of the steering wheel handle angle", this, SLOT(updateOverlaySize())); + property_background_color_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_background_alpha_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.3, "Background Color Alpha", this, SLOT(updateOverlayColor())); + property_primary_color_ = new rviz_common::properties::ColorProperty( + "Primary Color", QColor(174, 174, 174), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_light_limit_color_ = new rviz_common::properties::ColorProperty( + "Light Traffic Color", QColor(255, 153, 153), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_dark_limit_color_ = new rviz_common::properties::ColorProperty( + "Dark Traffic Color", QColor(255, 51, 51), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); // Initialize the component displays steering_wheel_display_ = std::make_unique(); @@ -112,9 +126,10 @@ void SignalDisplay::onInitialize() speed_limit_topic_property_->initialize(rviz_ros_node); traffic_topic_property_ = std::make_unique( - "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", - "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data", - this, SLOT(topic_updated_traffic())); + "Traffic Topic", + "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal", + "autoware_perception_msgs/msgs/msg/TrafficLightGroup", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); traffic_topic_property_->initialize(rviz_ros_node); } @@ -192,7 +207,7 @@ void SignalDisplay::onDisable() } void SignalDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg) { std::lock_guard lock(property_mutex_); @@ -279,12 +294,14 @@ void SignalDisplay::drawWidget(QImage & hud) QPainter painter(&hud); painter.setRenderHint(QPainter::Antialiasing, true); - QRectF backgroundRect(0, 0, 550, hud.height()); + QRectF backgroundRect(0, 0, hud.width(), hud.height()); drawHorizontalRoundedRectangle(painter, backgroundRect); // Draw components if (gear_display_) { - gear_display_->drawGearIndicator(painter, backgroundRect); + gear_display_->drawGearIndicator( + painter, backgroundRect, property_primary_color_->getColor(), + property_background_color_->getColor()); } if (steering_wheel_display_) { @@ -293,7 +310,7 @@ void SignalDisplay::drawWidget(QImage & hud) } if (speed_display_) { - speed_display_->drawSpeedDisplay(painter, backgroundRect); + speed_display_->drawSpeedDisplay(painter, backgroundRect, property_primary_color_->getColor()); } if (turn_signals_display_) { turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor()); @@ -304,7 +321,10 @@ void SignalDisplay::drawWidget(QImage & hud) } if (speed_limit_display_) { - speed_limit_display_->drawSpeedLimitIndicator(painter, backgroundRect); + speed_limit_display_->drawSpeedLimitIndicator( + painter, backgroundRect, property_primary_color_->getColor(), + property_light_limit_color_->getColor(), property_dark_limit_color_->getColor(), + property_background_color_->getColor(), property_background_alpha_->getFloat()); } painter.end(); @@ -315,9 +335,11 @@ void SignalDisplay::drawHorizontalRoundedRectangle( { painter.setRenderHint(QPainter::Antialiasing, true); QColor colorFromHSV; - colorFromHSV.setHsv(0, 0, 29); // Hue, Saturation, Value - colorFromHSV.setAlphaF(0.60); // Transparency - + colorFromHSV.setHsv( + property_background_color_->getColor().hue(), + property_background_color_->getColor().saturation(), + property_background_color_->getColor().value()); + colorFromHSV.setAlphaF(property_background_alpha_->getFloat()); painter.setBrush(colorFromHSV); painter.setPen(Qt::NoPen); @@ -328,8 +350,11 @@ void SignalDisplay::drawVerticalRoundedRectangle(QPainter & painter, const QRect { painter.setRenderHint(QPainter::Antialiasing, true); QColor colorFromHSV; - colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value - colorFromHSV.setAlphaF(0.65); // Transparency + colorFromHSV.setHsv( + property_background_color_->getColor().hue(), + property_background_color_->getColor().saturation(), + property_background_color_->getColor().value()); + colorFromHSV.setAlphaF(property_background_alpha_->getFloat()); painter.setBrush(colorFromHSV); @@ -458,14 +483,13 @@ void SignalDisplay::topic_updated_traffic() // resubscribe to the topic traffic_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - traffic_sub_ = - rviz_ros_node->get_raw_node() - ->create_subscription( - traffic_topic_property_->getTopicStd(), - rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), - [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) { - updateTrafficLightData(msg); - }); + traffic_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficLightGroup::SharedPtr msg) { + updateTrafficLightData(msg); + }); } } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index b9c22ec5f53ac..8670f85e6ba71 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -76,7 +76,8 @@ void SpeedDisplay::updateSpeedData( // } // } -void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect) +void SpeedDisplay::drawSpeedDisplay( + QPainter & painter, const QRectF & backgroundRect, const QColor & color) { QFont referenceFont("Quicksand", 80, QFont::Bold); painter.setFont(referenceFont); @@ -96,7 +97,7 @@ void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroun QPointF speedPos( backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y() + speedNumberRect.bottom()); - painter.setPen(gray); + painter.setPen(color); painter.drawText(speedPos, speedNumber); QFont unitFont("Quicksand", 8, QFont::DemiBold); diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp index 3dec6a8e7d4a0..a6691ffff1c62 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -68,41 +68,41 @@ void SpeedLimitDisplay::updateSpeedData( } } -void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect) +void SpeedLimitDisplay::drawSpeedLimitIndicator( + QPainter & painter, const QRectF & backgroundRect, const QColor & color, + const QColor & light_color, const QColor & dark_color, const QColor & bg_color, + const float bg_alpha) { // Enable Antialiasing for smoother drawing painter.setRenderHint(QPainter::Antialiasing, true); painter.setRenderHint(QPainter::SmoothPixmapTransform, true); - const double color_s_min = 0.4; - const double color_s_max = 0.8; - QColor colorMin; - colorMin.setHsvF(0.0, color_s_min, 1.0); - QColor colorMax; - colorMax.setHsvF(0.0, color_s_max, 1.0); + QColor borderColor = light_color; - QColor borderColor = colorMin; if (current_limit > 0.0) { double speed_to_limit_ratio = current_speed_ / current_limit; const double speed_to_limit_ratio_min = 0.6; const double speed_to_limit_ratio_max = 0.9; if (speed_to_limit_ratio >= speed_to_limit_ratio_max) { - borderColor = colorMax; + borderColor = dark_color; } else if (speed_to_limit_ratio > speed_to_limit_ratio_min) { double interpolation_factor = (speed_to_limit_ratio - speed_to_limit_ratio_min) / (speed_to_limit_ratio_max - speed_to_limit_ratio_min); - // Interpolate between colorMin and colorMax - double saturation = color_s_min + (color_s_max - color_s_min) * interpolation_factor; - - borderColor.setHsvF(0.0, saturation, 1.0); + // Interpolate between light_color and dark_color + int red = light_color.red() + (dark_color.red() - light_color.red()) * interpolation_factor; + int green = + light_color.green() + (dark_color.green() - light_color.green()) * interpolation_factor; + int blue = + light_color.blue() + (dark_color.blue() - light_color.blue()) * interpolation_factor; + borderColor = QColor(red, green, blue); } } // Define the area for the outer circle QRectF outerCircleRect = QRectF(45, 45, 45, 45); - outerCircleRect.moveTopRight( - QPointF(backgroundRect.right() - 44, backgroundRect.top() + outerCircleRect.height() / 2 + 5)); + outerCircleRect.moveTopRight(QPointF( + backgroundRect.right() - 44, backgroundRect.height() / 2 - outerCircleRect.height() / 2)); // Now use borderColor for drawing painter.setPen(QPen(borderColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); @@ -120,8 +120,8 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF painter.setRenderHint(QPainter::Antialiasing, true); QColor colorFromHSV; - colorFromHSV.setHsv(0, 0, 29); // Hue, Saturation, Value - colorFromHSV.setAlphaF(0.60); // Transparency + colorFromHSV.setHsv(bg_color.hue(), bg_color.saturation(), bg_color.value()); + colorFromHSV.setAlphaF(bg_alpha); painter.setBrush(colorFromHSV); painter.drawEllipse(innerCircleRect); @@ -138,8 +138,7 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF QFont font = QFont("Quicksand", 16, QFont::Bold); painter.setFont(font); - // #C2C2C2 - painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setPen(QPen(color, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); // Draw the text in the center of the circle painter.drawText(innerCircleRect, Qt::AlignCenter, text); diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index 2dc9c23583a52..dddd9b52f3d0b 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay() } void TrafficDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg) + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg) { current_traffic_ = *msg; } @@ -65,11 +65,11 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF QRectF circleRect = QRectF(50, 50, 50, 50); circleRect.moveTopRight(QPointF( backgroundRect.right() - circleRect.width() - 75, - backgroundRect.top() + circleRect.height() / 2)); + backgroundRect.height() / 2 - circleRect.height() / 2)); painter.drawEllipse(circleRect); - if (!current_traffic_.traffic_light_groups.empty()) { - switch (current_traffic_.traffic_light_groups[0].elements[0].color) { + if (!current_traffic_.elements.empty()) { + switch (current_traffic_.elements[0].color) { case 1: painter.setBrush(QBrush(tl_red_)); painter.drawEllipse(circleRect); diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml index 460186c33b7d8..dd2cd4222c422 100644 --- a/common/autoware_perception_rviz_plugin/package.xml +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -5,7 +5,6 @@ 1.0.0 Contains plugins to visualize object detection outputs Apex.AI, Inc. - Satoshi Tanaka Taiki Tanaka Takeshi Miura Shunsuke Miura 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 cd5e366c520fc..d49fced367908 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -46,9 +46,14 @@ 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: @@ -86,7 +91,8 @@ public: // 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)); + timer_ = + create_wall_timer(std::chrono::seconds(1), std::bind(&ExampleNode::func_a, this)); } private: @@ -100,6 +106,7 @@ private: // 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) @@ -109,6 +116,7 @@ private: { 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(); } @@ -116,6 +124,7 @@ private: { 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"); } }; @@ -133,9 +142,9 @@ int main(int argc, char ** argv) ```text ========================== - func_a (6.382ms) - └── func_b (5.243ms) - └── func_c (3.146ms) + 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`) @@ -145,16 +154,19 @@ int main(int argc, char ** argv) nodes: - id: 1 name: func_a - processing_time: 6.397 + processing_time: 6.366 parent_id: 0 + comment: This is a comment for func_a - id: 2 name: func_b - processing_time: 5.263 + processing_time: 5.237 parent_id: 1 + comment: This is a comment for func_b - id: 3 name: func_c - processing_time: 3.129 + processing_time: 3.156 parent_id: 2 + comment: This is a comment for func_c ``` #### `autoware::universe_utils::ScopedTimeTrack` 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_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp index c50aa1e1a8342..12f0b4f31e897 100644 --- a/common/autoware_universe_utils/examples/example_time_keeper.cpp +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -49,6 +49,7 @@ class ExampleNode : public rclcpp::Node // 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) @@ -58,6 +59,7 @@ class ExampleNode : public rclcpp::Node { 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(); } @@ -65,6 +67,7 @@ class ExampleNode : public rclcpp::Node { 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"); } }; 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 c635138416aa5..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 @@ -86,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 @@ -145,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 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 429f063dfc62e..88038575c70f5 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -43,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( @@ -70,6 +74,7 @@ tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const time_node_msg.processing_time = node.processing_time_; time_node_msg.id = static_cast(tree_msg.nodes.size() + 1); time_node_msg.parent_id = parent_id; + time_node_msg.comment = node.comment_; tree_msg.nodes.emplace_back(time_node_msg); for (const auto & child : node.child_nodes_) { @@ -93,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_; @@ -124,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/package.xml b/common/object_recognition_utils/package.xml index e7eaea96907f2..d0802bbc9b457 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -5,7 +5,6 @@ 0.1.0 The object_recognition_utils package Takayuki Murooka - Satoshi Tanaka Shunsuke Miura Yoshi Ri Apache License 2.0 diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp index 14c03a91d8fa1..55792beaaff11 100644 --- a/common/qp_interface/include/qp_interface/osqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -33,7 +33,7 @@ class OSQPInterface : public QPInterface public: /// \brief Constructor without problem formulation OSQPInterface( - const bool enable_warm_start = false, + const bool enable_warm_start = false, const int max_iteration = 20000, 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); diff --git a/common/qp_interface/include/qp_interface/proxqp_interface.hpp b/common/qp_interface/include/qp_interface/proxqp_interface.hpp index f75fb3da5954c..775ba0fcc42b8 100644 --- a/common/qp_interface/include/qp_interface/proxqp_interface.hpp +++ b/common/qp_interface/include/qp_interface/proxqp_interface.hpp @@ -31,8 +31,8 @@ class ProxQPInterface : public QPInterface { public: explicit ProxQPInterface( - const bool enable_warm_start, const double eps_abs, const double eps_rel, - const bool verbose = false); + const bool enable_warm_start, const int max_iteration, const double eps_abs, + const double eps_rel, const bool verbose = false); int getIterationNumber() const override; bool isSolved() const override; diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp index 0e8cb7e13988a..3ada25e8fa023 100644 --- a/common/qp_interface/src/osqp_interface.cpp +++ b/common/qp_interface/src/osqp_interface.cpp @@ -21,8 +21,8 @@ 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) + const bool enable_warm_start, const int max_iteration, 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(); @@ -35,8 +35,8 @@ OSQPInterface::OSQPInterface( 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_->warm_start = enable_warm_start; + settings_->max_iter = max_iteration; settings_->verbose = verbose; settings_->polish = polish; } diff --git a/common/qp_interface/src/proxqp_interface.cpp b/common/qp_interface/src/proxqp_interface.cpp index 507f3858cbf1b..d26543f907e62 100644 --- a/common/qp_interface/src/proxqp_interface.cpp +++ b/common/qp_interface/src/proxqp_interface.cpp @@ -19,9 +19,11 @@ namespace autoware::common using proxsuite::proxqp::QPSolverOutput; ProxQPInterface::ProxQPInterface( - const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose) + const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, + const bool verbose) : QPInterface(enable_warm_start) { + settings_.max_iter = max_iteration; settings_.eps_abs = eps_abs; settings_.eps_rel = eps_rel; settings_.verbose = verbose; diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp index 9a02dd4b00934..4d0a88c50475c 100644 --- a/common/qp_interface/test/test_osqp_interface.cpp +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -63,7 +63,7 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during optimization - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); @@ -72,7 +72,7 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during initialization - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); const auto status = osqp.getStatus(); const auto polish_status = osqp.getPolishStatus(); @@ -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); - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); } @@ -95,7 +95,7 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); const auto status = osqp.getStatus(); @@ -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); - autoware::common::OSQPInterface osqp(false, 1e-6); + autoware::common::OSQPInterface osqp(false, 4000, 1e-6); osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization @@ -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); - autoware::common::OSQPInterface osqp(true, 1e-6); // enable warm start + autoware::common::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); // Redefine problem before optimization diff --git a/common/qp_interface/test/test_proxqp_interface.cpp b/common/qp_interface/test/test_proxqp_interface.cpp index a8b1bb29dafa0..e9cb90ef5d442 100644 --- a/common/qp_interface/test/test_proxqp_interface.cpp +++ b/common/qp_interface/test/test_proxqp_interface.cpp @@ -57,7 +57,7 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization - autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false); + autoware::common::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); const auto status = proxqp.getStatus(); check_result(solution, status); @@ -65,7 +65,7 @@ TEST(TestProxqpInterface, BasicQp) { // Define problem during optimization with warm start - autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false); + autoware::common::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); { const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); const auto status = proxqp.getStatus(); diff --git a/common/tensorrt_common/src/simple_profiler.cpp b/common/tensorrt_common/src/simple_profiler.cpp index fbad1b6a40cc3..b9400c284c2eb 100644 --- a/common/tensorrt_common/src/simple_profiler.cpp +++ b/common/tensorrt_common/src/simple_profiler.cpp @@ -22,7 +22,6 @@ namespace tensorrt_common SimpleProfiler::SimpleProfiler(std::string name, const std::vector & src_profilers) : m_name(name) { - float total_time = 0.0; m_index = 0; for (const auto & src_profiler : src_profilers) { for (const auto & rec : src_profiler.m_profile) { @@ -32,7 +31,6 @@ SimpleProfiler::SimpleProfiler(std::string name, const std::vectorsecond.time += rec.second.time; it->second.count += rec.second.count; - total_time += rec.second.time; } } } diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index a22372f74a286..87e8f422dd3f8 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -278,7 +278,6 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) for (int i = 0; i < num; i++) { nvinfer1::ILayer * layer = network->getLayer(i); auto layer_type = layer->getType(); - std::string name = layer->getName(); if (build_config_->profile_per_layer) { model_profiler_.setProfDict(layer); } diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index 1c1c4ad6d8ba1..291c7ff1617cc 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -6,7 +6,10 @@ The tier4_localization_rviz_plugin package Takagi, Isamu Takamasa Horibe + Yamato Ando Apache License 2.0 + Takagi, Isamu + Takamasa Horibe ament_cmake_auto autoware_cmake diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index 0c64b9498486a..d0c75f893cd2d 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -18,6 +18,7 @@ autoware_control_msgs autoware_motion_utils autoware_planning_msgs + autoware_pointcloud_preprocessor autoware_system_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -27,7 +28,6 @@ nav_msgs pcl_conversions pcl_ros - pointcloud_preprocessor rclcpp rclcpp_components sensor_msgs diff --git a/control/autoware_control_validator/CMakeLists.txt b/control/autoware_control_validator/CMakeLists.txt index 2b25b5b984c12..785fb8472ca5a 100644 --- a/control/autoware_control_validator/CMakeLists.txt +++ b/control/autoware_control_validator/CMakeLists.txt @@ -36,8 +36,13 @@ else() target_link_libraries(autoware_control_validator_component "${cpp_typesupport_target}") endif() -# if(BUILD_TESTING) -# endif() +if(BUILD_TESTING) + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_gtest(test_autoware_control_validator + ${TEST_SOURCES} + ) + target_link_libraries(test_autoware_control_validator autoware_control_validator_component) +endif() ament_auto_package( INSTALL_TO_SHARE 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..56a1e39391b10 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 @@ -17,18 +17,21 @@ #include "autoware/control_validator/debug_marker.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "autoware_control_validator/msg/control_validator_status.hpp" -#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "autoware_vehicle_info_utils/vehicle_info.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" -#include +#include #include #include #include #include +#include #include #include +#include +#include namespace autoware::control_validator { @@ -42,42 +45,107 @@ 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 + * @brief Validates control commands by comparing predicted trajectories against reference + * trajectories. + */ class ControlValidator : public rclcpp::Node { public: + /** + * @brief Constructor + * @param options Node options + */ explicit ControlValidator(const rclcpp::NodeOptions & options); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); - - bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); + /** + * @brief Callback function for the predicted trajectory. + * @param msg Predicted trajectory message + */ + void on_predicted_trajectory(const Trajectory::ConstSharedPtr msg); + + /** + * @brief Calculate the maximum lateral distance between the reference trajectory and predicted + * trajectory. + * @param predicted_trajectory Predicted trajectory + * @param reference_trajectory Reference trajectory + * @return A pair consisting of the maximum lateral deviation and a boolean indicating validity + */ + std::pair calc_lateral_deviation_status( + const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const; + + /** + * @brief Calculate the velocity deviation between the reference trajectory and the current + * vehicle kinematics. + * @param reference_trajectory Reference trajectory + * @param kinematics Current vehicle kinematics + * @return A tuple containing the current velocity, desired velocity, and a boolean indicating + * validity + */ + std::tuple calc_velocity_deviation_status( + const Trajectory & reference_trajectory, const Odometry & kinematics) const; private: - void setupDiag(); - - void setupParameters(); - - bool isDataReady(); - - void validate(const Trajectory & trajectory); - - void publishPredictedTrajectory(); - void publishDebugInfo(); - void displayStatus(); - - void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - + /** + * @brief Setup diagnostic updater + */ + void setup_diag(); + + /** + * @brief Setup parameters from the parameter server + */ + void setup_parameters(); + + /** + * @brief Check if all required data is ready for validation + * @return Boolean indicating readiness of data + */ + bool is_data_ready(); + + /** + * @brief Validate the predicted trajectory against the reference trajectory and current + * kinematics + * @param predicted_trajectory Predicted trajectory + * @param reference_trajectory Reference trajectory + * @param kinematics Current vehicle kinematics + */ + void validate( + const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory, + const Odometry & kinematics); + + /** + * @brief Publish debug information + */ + void publish_debug_info(); + + /** + * @brief Display validation status on terminal + */ + void display_status(); + + /** + * @brief Set the diagnostic status + * @param stat Diagnostic status wrapper + * @param is_ok Boolean indicating if the status is okay + * @param msg Status message + */ + void set_status( + DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const; + + // Subscribers and publishers rclcpp::Subscription::SharedPtr sub_predicted_traj_; - autoware::universe_utils::InterProcessPollingSubscriber sub_kinematics_{ - this, "~/input/kinematics"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_reference_traj_{ - this, "~/input/reference_trajectory"}; + universe_utils::InterProcessPollingSubscriber::SharedPtr sub_kinematics_; + universe_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; // system parameters - int diag_error_count_threshold_ = 0; + int64_t diag_error_count_threshold_ = 0; bool display_on_terminal_ = true; Updater diag_updater_{this}; @@ -85,9 +153,14 @@ class ControlValidator : public rclcpp::Node ControlValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + vehicle_info_utils::VehicleInfo vehicle_info_; - bool isAllValid(const ControlValidatorStatus & status); + /** + * @brief Check if all validation criteria are met + * @param status Validation status + * @return Boolean indicating if all criteria are met + */ + static bool is_all_valid(const ControlValidatorStatus & status); Trajectory::ConstSharedPtr current_reference_trajectory_; Trajectory::ConstSharedPtr current_predicted_trajectory_; diff --git a/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp index c03ecb46b8117..4c084ecbcc3e8 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/debug_marker.hpp @@ -22,23 +22,41 @@ #include #include -#include #include -#include +/** + * @brief Class for publishing debug markers + */ class ControlValidatorDebugMarkerPublisher { public: + /** + * @brief Constructor + */ explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); - void pushPoseMarker( - const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); - void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); - void pushVirtualWall(const geometry_msgs::msg::Pose & pose); - void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); + /** + * @brief Push a virtual wall + * @param pose pose of the virtual wall + */ + void push_virtual_wall(const geometry_msgs::msg::Pose & pose); + + /** + * @brief Push a warning message + * @param pose pose of the warning message + * @param msg warning message + */ + void push_warning_msg(const geometry_msgs::msg::Pose & pose, const std::string & msg); + + /** + * @brief Publish markers + */ void publish(); - void clearMarkers(); + /** + * @brief Clear markers + */ + void clear_markers(); private: rclcpp::Node * node_; @@ -47,14 +65,6 @@ class ControlValidatorDebugMarkerPublisher rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr virtual_wall_pub_; std::map marker_id_; - - int getMarkerId(const std::string & ns) - { - if (marker_id_.count(ns) == 0) { - marker_id_[ns] = 0.0; - } - return marker_id_[ns]++; - } }; #endif // AUTOWARE__CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp index 375ad557d02df..85ee1f44bd457 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/utils.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/utils.hpp @@ -15,41 +15,26 @@ #ifndef AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ #define AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ -#include #include #include -#include -#include -#include - namespace autoware::control_validator { -using autoware::motion_utils::convertToTrajectory; -using autoware::motion_utils::convertToTrajectoryPointArray; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::Pose; -using TrajectoryPoints = std::vector; - -void shiftPose(Pose & pose, double longitudinal); - -void insertPointInPredictedTrajectory( - TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose, - const TrajectoryPoints & predicted_trajectory); - -TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory); - -bool removeFrontTrajectoryPoint( - const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, - const TrajectoryPoints & predicted_trajectory_points); - -Trajectory alignTrajectoryWithReferenceTrajectory( - const Trajectory & trajectory, const Trajectory & predicted_trajectory); - -double calcMaxLateralDistance( - const Trajectory & trajectory, const Trajectory & predicted_trajectory); +/** + * @brief Shift pose along the yaw direction + */ +void shift_pose(geometry_msgs::msg::Pose & pose, double longitudinal); + +/** + * @brief Calculate the maximum lateral distance between the reference trajectory and the predicted + * trajectory + * @param reference_trajectory reference trajectory + * @param predicted_trajectory predicted trajectory + */ +double calc_max_lateral_distance( + const autoware_planning_msgs::msg::Trajectory & reference_trajectory, + const autoware_planning_msgs::msg::Trajectory & predicted_trajectory); } // namespace autoware::control_validator #endif // AUTOWARE__CONTROL_VALIDATOR__UTILS_HPP_ 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/package.xml b/control/autoware_control_validator/package.xml index 71fefafbaffb1..b48ebef70bc58 100644 --- a/control/autoware_control_validator/package.xml +++ b/control/autoware_control_validator/package.xml @@ -34,6 +34,7 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils rosidl_default_runtime rosidl_interface_packages diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 4c8ca3c831824..2370d274fecc8 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -15,55 +15,73 @@ #include "autoware/control_validator/control_validator.hpp" #include "autoware/control_validator/utils.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include + +#include #include #include -#include namespace autoware::control_validator { using diagnostic_msgs::msg::DiagnosticStatus; ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) -: Node("control_validator", options) +: Node("control_validator", options), validation_params_(), vehicle_info_() { using std::placeholders::_1; + sub_predicted_traj_ = create_subscription( "~/input/predicted_trajectory", 1, - std::bind(&ControlValidator::onPredictedTrajectory, this, _1)); + std::bind(&ControlValidator::on_predicted_trajectory, this, _1)); + sub_kinematics_ = + universe_utils::InterProcessPollingSubscriber::create_subscription( + this, "~/input/kinematics", 1); + sub_reference_traj_ = + autoware::universe_utils::InterProcessPollingSubscriber::create_subscription( + this, "~/input/reference_trajectory", 1); pub_status_ = create_publisher("~/output/validation_status", 1); + pub_markers_ = create_publisher("~/output/markers", 1); debug_pose_publisher_ = std::make_shared(this); - setupParameters(); + setup_parameters(); - setupDiag(); + setup_diag(); } -void ControlValidator::setupParameters() +void ControlValidator::setup_parameters() { - diag_error_count_threshold_ = declare_parameter("diag_error_count_threshold"); + diag_error_count_threshold_ = declare_parameter("diag_error_count_threshold"); display_on_terminal_ = declare_parameter("display_on_terminal"); { 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 { vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); } catch (...) { - RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); vehicle_info_.front_overhang_m = 0.5; vehicle_info_.wheel_base_m = 4.0; + RCLCPP_ERROR( + get_logger(), + "failed to get vehicle info. use default value. vehicle_info_.front_overhang_m: %.2f, " + "vehicle_info_.wheel_base_m: %.2f", + vehicle_info_.front_overhang_m, vehicle_info_.wheel_base_m); } } -void ControlValidator::setStatus( - DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) +void ControlValidator::set_status( + DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) const { if (is_ok) { stat.summary(DiagnosticStatus::OK, "validated."); @@ -77,72 +95,78 @@ void ControlValidator::setStatus( } } -void ControlValidator::setupDiag() +void ControlValidator::setup_diag() { auto & d = diag_updater_; d.setHardwareID("control_validator"); std::string ns = "control_validation_"; d.add(ns + "max_distance_deviation", [&](auto & stat) { - setStatus( + set_status( stat, validation_status_.is_valid_max_distance_deviation, "control output is deviated from trajectory"); }); + d.add(ns + "velocity_deviation", [&](auto & stat) { + set_status( + stat, validation_status_.is_valid_velocity_deviation, + "current velocity is deviated from the desired velocity"); + }); } -bool ControlValidator::isDataReady() +bool ControlValidator::is_data_ready() { - const auto waiting = [this](const auto s) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", s); + const auto waiting = [this](const auto topic_name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", topic_name); return false; }; if (!current_kinematics_) { - return waiting("current_kinematics_"); + return waiting(sub_kinematics_->subscriber()->get_topic_name()); } if (!current_reference_trajectory_) { - return waiting("current_reference_trajectory_"); + return waiting(sub_reference_traj_->subscriber()->get_topic_name()); } if (!current_predicted_trajectory_) { - return waiting("current_predicted_trajectory_"); + return waiting(sub_predicted_traj_->get_topic_name()); } return true; } -void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) +void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg) { current_predicted_trajectory_ = msg; - current_reference_trajectory_ = sub_reference_traj_.takeData(); - current_kinematics_ = sub_kinematics_.takeData(); + current_reference_trajectory_ = sub_reference_traj_->takeData(); + current_kinematics_ = sub_kinematics_->takeData(); - if (!isDataReady()) return; + if (!is_data_ready()) return; - debug_pose_publisher_->clearMarkers(); + debug_pose_publisher_->clear_markers(); - validate(*current_predicted_trajectory_); + validate(*current_predicted_trajectory_, *current_reference_trajectory_, *current_kinematics_); diag_updater_.force_update(); // for debug - publishDebugInfo(); - displayStatus(); + publish_debug_info(); + display_status(); } -void ControlValidator::publishDebugInfo() +void ControlValidator::publish_debug_info() { - validation_status_.stamp = get_clock()->now(); pub_status_->publish(validation_status_); - if (!isAllValid(validation_status_)) { + if (!is_all_valid(validation_status_)) { geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose; - shiftPose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); - debug_pose_publisher_->pushVirtualWall(front_pose); - debug_pose_publisher_->pushWarningMsg(front_pose, "INVALID CONTROL"); + shift_pose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); + debug_pose_publisher_->push_virtual_wall(front_pose); + debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); } 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( @@ -150,32 +174,63 @@ void ControlValidator::validate(const Trajectory & predicted_trajectory) "predicted_trajectory size is less than 2. Cannot validate."); return; } + if (reference_trajectory.points.size() < 2) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "reference_trajectory size is less than 2. Cannot validate."); + return; + } - auto & s = validation_status_; + validation_status_.stamp = get_clock()->now(); + + std::tie( + validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) = + calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_); - s.is_valid_max_distance_deviation = checkValidMaxDistanceDeviation(predicted_trajectory); + std::tie( + validation_status_.current_velocity, validation_status_.desired_velocity, + validation_status_.is_valid_velocity_deviation) = + calc_velocity_deviation_status(*current_reference_trajectory_, kinematics); - s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; + validation_status_.invalid_count = + is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1; } -bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory) +std::pair ControlValidator::calc_lateral_deviation_status( + const Trajectory & predicted_trajectory, const Trajectory & reference_trajectory) const { - validation_status_.max_distance_deviation = - calcMaxLateralDistance(*current_reference_trajectory_, predicted_trajectory); - if ( - validation_status_.max_distance_deviation > - validation_params_.max_distance_deviation_threshold) { - return false; - } - return true; + auto max_distance_deviation = + calc_max_lateral_distance(reference_trajectory, predicted_trajectory); + return { + max_distance_deviation, + max_distance_deviation <= validation_params_.max_distance_deviation_threshold}; +} + +std::tuple ControlValidator::calc_velocity_deviation_status( + const Trajectory & reference_trajectory, const Odometry & kinematics) const +{ + const double current_vel = kinematics.twist.twist.linear.x; + const double desired_vel = + autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose) + .longitudinal_velocity_mps; + + 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 {current_vel, desired_vel, !(is_over_velocity || is_reverse_velocity)}; } -bool ControlValidator::isAllValid(const ControlValidatorStatus & s) +bool ControlValidator::is_all_valid(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() +void ControlValidator::display_status() { if (!display_on_terminal_) return; rclcpp::Clock clock{RCL_ROS_TIME}; diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp index d5d8644515ab1..dfd073884ce89 100644 --- a/control/autoware_control_validator/src/debug_marker.cpp +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -17,9 +17,7 @@ #include #include -#include #include -#include using visualization_msgs::msg::Marker; @@ -33,47 +31,13 @@ ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcp node_->create_publisher("~/virtual_wall", 1); } -void ControlValidatorDebugMarkerPublisher::clearMarkers() +void ControlValidatorDebugMarkerPublisher::clear_markers() { marker_array_.markers.clear(); marker_array_virtual_wall_.markers.clear(); } -void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) -{ - pushPoseMarker(p.pose, ns, id); -} - -void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) -{ - using autoware::universe_utils::createMarkerColor; - - // append arrow marker - std_msgs::msg::ColorRGBA color; - if (id == 0) // Red - { - color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - } - if (id == 1) // Green - { - color = createMarkerColor(0.0, 1.0, 0.0, 0.999); - } - if (id == 2) // Blue - { - color = createMarkerColor(0.0, 0.0, 1.0, 0.999); - } - Marker marker = autoware::universe_utils::createDefaultMarker( - "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - autoware::universe_utils::createMarkerScale(0.2, 0.1, 0.3), color); - marker.lifetime = rclcpp::Duration::from_seconds(0.2); - marker.pose = pose; - - marker_array_.markers.push_back(marker); -} - -void ControlValidatorDebugMarkerPublisher::pushWarningMsg( +void ControlValidatorDebugMarkerPublisher::push_warning_msg( const geometry_msgs::msg::Pose & pose, const std::string & msg) { visualization_msgs::msg::Marker marker = autoware::universe_utils::createDefaultMarker( @@ -86,7 +50,7 @@ void ControlValidatorDebugMarkerPublisher::pushWarningMsg( marker_array_virtual_wall_.markers.push_back(marker); } -void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) +void ControlValidatorDebugMarkerPublisher::push_virtual_wall(const geometry_msgs::msg::Pose & pose) { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp index 821d64c3af6e6..12f4ea70f2ca1 100644 --- a/control/autoware_control_validator/src/utils.cpp +++ b/control/autoware_control_validator/src/utils.cpp @@ -14,26 +14,37 @@ #include "autoware/control_validator/utils.hpp" -#include -#include -#include +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" -#include -#include -#include #include namespace autoware::control_validator { -void shiftPose(Pose & pose, double longitudinal) +using autoware::motion_utils::convertToTrajectory; +using autoware::motion_utils::convertToTrajectoryPointArray; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using TrajectoryPoints = std::vector; + +void shift_pose(Pose & pose, double longitudinal) { const auto yaw = tf2::getYaw(pose.orientation); pose.position.x += std::cos(yaw) * longitudinal; pose.position.y += std::sin(yaw) * longitudinal; } -void insertPointInPredictedTrajectory( +/** + * @brief Insert interpolated point along the predicted_trajectory to the modified_trajectory + * @param[inout] modified_trajectory modified trajectory + * @param[in] reference_pose reference pose + * @param[in] predicted_trajectory predicted trajectory + */ +void insert_point_in_predicted_trajectory( TrajectoryPoints & modified_trajectory, const Pose & reference_pose, const TrajectoryPoints & predicted_trajectory) { @@ -42,7 +53,7 @@ void insertPointInPredictedTrajectory( modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); } -TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) +TrajectoryPoints reverse_trajectory_points(const TrajectoryPoints & trajectory_points) { TrajectoryPoints reversed_trajectory_points; reversed_trajectory_points.reserve(trajectory_points.size()); @@ -52,7 +63,7 @@ TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_poi return reversed_trajectory_points; } -bool removeFrontTrajectoryPoint( +bool remove_front_trajectory_point( const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, const TrajectoryPoints & predicted_trajectory_points) { @@ -72,7 +83,7 @@ bool removeFrontTrajectoryPoint( return predicted_trajectory_point_removed; } -Trajectory alignTrajectoryWithReferenceTrajectory( +Trajectory align_trajectory_with_reference_trajectory( const Trajectory & trajectory, const Trajectory & predicted_trajectory) { const auto last_seg_length = autoware::motion_utils::calcSignedArcLength( @@ -109,11 +120,11 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // ↓ // predicted_trajectory: pNew--p3----//------pN // trajectory: t1--------//------tN - auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + auto predicted_trajectory_point_removed = remove_front_trajectory_point( trajectory_points, modified_trajectory_points, predicted_trajectory_points); if (predicted_trajectory_point_removed) { - insertPointInPredictedTrajectory( + insert_point_in_predicted_trajectory( modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points); } @@ -125,28 +136,29 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // predicted_trajectory: p1-----//------pN-2-pNew // trajectory: t1-----//-----tN-1--tN - auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points); - auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points); - auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points); + auto reversed_predicted_trajectory_points = + reverse_trajectory_points(predicted_trajectory_points); + auto reversed_trajectory_points = reverse_trajectory_points(trajectory_points); + auto reversed_modified_trajectory_points = reverse_trajectory_points(modified_trajectory_points); - auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + auto reversed_predicted_trajectory_point_removed = remove_front_trajectory_point( reversed_trajectory_points, reversed_modified_trajectory_points, reversed_predicted_trajectory_points); if (reversed_predicted_trajectory_point_removed) { - insertPointInPredictedTrajectory( + insert_point_in_predicted_trajectory( reversed_modified_trajectory_points, reversed_trajectory_points.front().pose, reversed_predicted_trajectory_points); } - return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points)); + return convertToTrajectory(reverse_trajectory_points(reversed_modified_trajectory_points)); } -double calcMaxLateralDistance( +double calc_max_lateral_distance( const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory) { const auto alined_predicted_trajectory = - alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); + align_trajectory_with_reference_trajectory(reference_trajectory, predicted_trajectory); double max_dist = 0; for (const auto & point : alined_predicted_trajectory.points) { const auto p0 = autoware::universe_utils::getPoint(point); diff --git a/control/autoware_control_validator/test/test_control_validator.cpp b/control/autoware_control_validator/test/test_control_validator.cpp new file mode 100644 index 0000000000000..c4d5b04fa920c --- /dev/null +++ b/control/autoware_control_validator/test/test_control_validator.cpp @@ -0,0 +1,170 @@ +// 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/control_validator/control_validator.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +Trajectory make_linear_trajectory( + const TrajectoryPoint & start, const TrajectoryPoint & end, size_t num_points, double velocity) +{ + auto create_quaternion = [](double yaw) { + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); + }; + + double yaw = std::atan2( + end.pose.position.y - start.pose.position.y, end.pose.position.x - start.pose.position.x); + yaw += (velocity < 0) ? M_PI : 0; + + Trajectory trajectory; + trajectory.points.reserve(num_points); + + for (size_t i = 0; i < num_points; ++i) { + double ratio = static_cast(i) / static_cast(num_points - 1); + + TrajectoryPoint point; + point.pose.position.x = + start.pose.position.x + ratio * (end.pose.position.x - start.pose.position.x); + point.pose.position.y = + start.pose.position.y + ratio * (end.pose.position.y - start.pose.position.y); + point.pose.orientation = create_quaternion(yaw); + point.longitudinal_velocity_mps = static_cast(velocity); + point.lateral_velocity_mps = 0.0; + + trajectory.points.emplace_back(point); + } + + return trajectory; +} + +TrajectoryPoint make_trajectory_point(double x, double y) +{ + TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + return point; +} + +class ControlValidatorTest +: public ::testing::TestWithParam> +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.arguments( + {"--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_control_validator") + + "/config/control_validator.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml"}); + + node = std::make_shared(options); + } + + void TearDown() override { rclcpp::shutdown(); } + + std::shared_ptr node; +}; + +TEST_P(ControlValidatorTest, test_calc_lateral_deviation_status) +{ + auto [reference_trajectory, predicted_trajectory, expected_deviation, expected_condition] = + GetParam(); + auto [deviation, is_valid] = + node->calc_lateral_deviation_status(predicted_trajectory, reference_trajectory); + + EXPECT_EQ(is_valid, expected_condition); + EXPECT_NEAR(deviation, expected_deviation, 1e-5); +} + +INSTANTIATE_TEST_SUITE_P( + ControlValidatorTests, ControlValidatorTest, + ::testing::Values( + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0.99), 11, 1.0), + 0.99, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.0), 11, 1.0), + 1.0, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.01), 11, 1.0), + 1.01, false), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0), + make_linear_trajectory( + make_trajectory_point(0, 0), make_trajectory_point(10, 0.99), 11, -1.0), + 0.99, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0), + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 1.0), 11, -1.0), + 1.0, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, -1.0), + make_linear_trajectory( + make_trajectory_point(0, 0), make_trajectory_point(10, 1.01), 11, -1.0), + 1.01, false), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(11, 0), make_trajectory_point(20, 0.0), 11, 1.0), + 0.0, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(11, 0), make_trajectory_point(20, 0.0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + 0.0, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(1, 0), make_trajectory_point(10, 1.0), 11, 1.0), + 1.0, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(-1, 0), make_trajectory_point(10, 1.0), 11, 1.0), + 1.0, true), + + std::make_tuple( + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(10, 0), 11, 1.0), + make_linear_trajectory(make_trajectory_point(0, 0), make_trajectory_point(20, 2.0), 21, 1.0), + 1.0, true)) + +); diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index b43a7d0d0369a..7e39cb6cba8c4 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -101,6 +102,12 @@ struct Output class LaneDepartureChecker { public: + LaneDepartureChecker( + std::shared_ptr time_keeper = + std::make_shared()) + : time_keeper_(time_keeper) + { + } Output update(const Input & input); void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) @@ -156,9 +163,9 @@ class LaneDepartureChecker static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); - static bool willLeaveLane( + bool willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints); + const std::vector & vehicle_footprints) const; double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const; @@ -166,9 +173,11 @@ class LaneDepartureChecker const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, const double max_search_length, const std::vector & boundary_types_to_detect); - static bool willCrossBoundary( + bool willCrossBoundary( const std::vector & vehicle_footprints, - const SegmentRtree & uncrossable_segments); + const SegmentRtree & uncrossable_segments) const; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::lane_departure_checker 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.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 132eba8dde0c9..fcb64607f6d46 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -160,6 +160,8 @@ Output LaneDepartureChecker::update(const Input & input) bool LaneDepartureChecker::checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::vector vehicle_footprints = createVehicleFootprints(path); lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints); return willLeaveLane(candidate_lanelets, vehicle_footprints); @@ -290,8 +292,10 @@ std::vector LaneDepartureChecker::createVehiclePassingAreas( bool LaneDepartureChecker::willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints) + const std::vector & vehicle_footprints) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + for (const auto & vehicle_footprint : vehicle_footprints) { if (isOutOfLane(candidate_lanelets, vehicle_footprint)) { return true; @@ -304,6 +308,8 @@ bool LaneDepartureChecker::willLeaveLane( std::vector> LaneDepartureChecker::getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // Get Footprint Hull basic polygon std::vector vehicle_footprints = createVehicleFootprints(path); LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); @@ -326,18 +332,20 @@ std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> autoware::universe_utils::Polygon2d { - autoware::universe_utils::Polygon2d p; - auto & outer = p.outer(); + autoware::universe_utils::Polygon2d polygon; + auto & outer = polygon.outer(); for (const auto & p : poly) { autoware::universe_utils::Point2d p2d(p.x(), p.y()); outer.push_back(p2d); } - boost::geometry::correct(p); - return p; + boost::geometry::correct(polygon); + return polygon; }; // Fuse lanelets into a single BasicPolygon2d @@ -365,6 +373,8 @@ LaneDepartureChecker::getFusedLaneletPolygonForPath( bool LaneDepartureChecker::checkPathWillLeaveLane( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // check if the footprint is not fully contained within the fused lanelets polygon const std::vector vehicle_footprints = createVehicleFootprints(path); const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); @@ -379,17 +389,26 @@ bool LaneDepartureChecker::checkPathWillLeaveLane( PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + PathWithLaneId temp_path; const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); if (path.points.empty() || !fused_lanelets_polygon) return temp_path; const auto vehicle_footprints = createVehicleFootprints(path); - size_t idx = 0; - std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { - if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { - temp_path.points.push_back(path.points.at(idx)); - } - ++idx; - }); + + { + universe_utils::ScopedTimeTrack st2( + "check if footprint is within fused_lanlets_polygon", *time_keeper_); + + size_t idx = 0; + std::for_each( + vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { + if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { + temp_path.points.push_back(path.points.at(idx)); + } + ++idx; + }); + } PathWithLaneId cropped_path = path; cropped_path.points = temp_path.points; return cropped_path; @@ -449,8 +468,11 @@ SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries( } bool LaneDepartureChecker::willCrossBoundary( - const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments) + const std::vector & vehicle_footprints, + const SegmentRtree & uncrossable_segments) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + for (const auto & footprint : vehicle_footprints) { std::vector intersection_result; uncrossable_segments.query( 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_mpc_lateral_controller/src/lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp index bd90b2aa896f6..9ed65536da2c6 100644 --- a/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp @@ -63,11 +63,9 @@ void Butterworth2dFilter::filt_vector(const std::vector & t, std::vector double y2 = t.at(0); double u2 = t.at(0); double u1 = t.at(0); - double y0 = 0.0; - double u0 = 0.0; for (size_t i = 0; i < t.size(); ++i) { - u0 = t.at(i); - y0 = m_b2 * u2 + m_b1 * u1 + m_b0 * u0 + m_a2 * y2 + m_a1 * y1; + const double u0 = t.at(i); + const double y0 = m_b2 * u2 + m_b1 * u1 + m_b0 * u0 + m_a2 * y2 + m_a1 * y1; y2 = y1; y1 = y0; u2 = u1; diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index dfc4d82541bf8..1682f4ddc56c7 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -804,8 +804,10 @@ Trajectory MPC::calculatePredictedTrajectory( const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate( mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, dt); - const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( + auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( MPCUtils::clipTrajectoryByLength(frenet, predicted_length)); + frenet_clipped.header.stamp = m_clock->now(); + frenet_clipped.header.frame_id = "map"; m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped); } diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 43acbb862b573..0eef731bacd18 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -581,11 +581,12 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( // strong exception safety wrt MPCParam MPCParam param = m_mpc->m_param; - auto & nw = param.nominal_weight; - auto & lcw = param.low_curvature_weight; using MPCUtils::update_param; try { + auto & nw = param.nominal_weight; + auto & lcw = param.low_curvature_weight; + update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); update_param(parameters, "mpc_prediction_dt", param.prediction_dt); 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_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 0fd85b5822924..57999372dceed 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -184,7 +184,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro { double vel; double acc; - double jerk; }; StoppedStateParams m_stopped_state_params; @@ -204,6 +203,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // jerk limit double m_max_jerk; double m_min_jerk; + double m_max_acc_cmd_diff; // slope compensation enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE }; @@ -292,7 +292,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @brief calculate control command in emergency state * @param [in] dt time between previous and current one */ - Motion calcEmergencyCtrlCmd(const double dt) const; + Motion calcEmergencyCtrlCmd(const double dt); /** * @brief update control state according to the current situation diff --git a/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index ad6217663fbae..5027c94afe7c1 100644 --- a/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -53,12 +53,11 @@ # stopped state stopped_vel: 0.0 - stopped_acc: -3.4 - stopped_jerk: -5.0 + stopped_acc: -3.4 # denotes pedal position # emergency state emergency_vel: 0.0 - emergency_acc: -5.0 + emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 # acceleration limit @@ -68,6 +67,7 @@ # jerk limit max_jerk: 2.0 min_jerk: -5.0 + max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1] # slope compensation lpf_pitch_gain: 0.95 diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index a0118092413a9..9190c065eca3c 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -152,9 +152,8 @@ PidLongitudinalController::PidLongitudinalController( // parameters for stop state { auto & p = m_stopped_state_params; - p.vel = node.declare_parameter("stopped_vel"); // [m/s] - p.acc = node.declare_parameter("stopped_acc"); // [m/s^2] - p.jerk = node.declare_parameter("stopped_jerk"); // [m/s^3] + p.vel = node.declare_parameter("stopped_vel"); // [m/s] + p.acc = node.declare_parameter("stopped_acc"); // [m/s^2] } // parameters for emergency state @@ -170,8 +169,9 @@ PidLongitudinalController::PidLongitudinalController( m_min_acc = node.declare_parameter("min_acc"); // [m/s^2] // parameters for jerk limit - m_max_jerk = node.declare_parameter("max_jerk"); // [m/s^3] - m_min_jerk = node.declare_parameter("min_jerk"); // [m/s^3] + m_max_jerk = node.declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = node.declare_parameter("min_jerk"); // [m/s^3] + m_max_acc_cmd_diff = node.declare_parameter("max_acc_cmd_diff"); // [m/s^3] // parameters for slope compensation m_adaptive_trajectory_velocity_th = @@ -355,7 +355,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac auto & p = m_stopped_state_params; update_param("stopped_vel", p.vel); update_param("stopped_acc", p.acc); - update_param("stopped_jerk", p.jerk); } // emergency state @@ -372,6 +371,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac // jerk limit update_param("max_jerk", m_max_jerk); update_param("min_jerk", m_min_jerk); + update_param("max_acc_cmd_diff", m_max_acc_cmd_diff); // slope compensation update_param("max_pitch_rad", m_max_pitch_rad); @@ -567,26 +567,30 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData return control_data; } -PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd( - const double dt) const +PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd(const double dt) { // These accelerations are without slope compensation const auto & p = m_emergency_state_params; - const double vel = - longitudinal_utils::applyDiffLimitFilter(p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); - const double acc = - longitudinal_utils::applyDiffLimitFilter(p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); + Motion raw_ctrl_cmd{p.vel, p.acc}; + + raw_ctrl_cmd.vel = + longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); + raw_ctrl_cmd.acc = std::clamp(raw_ctrl_cmd.acc, m_min_acc, m_max_acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, raw_ctrl_cmd.acc); + raw_ctrl_cmd.acc = + longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc); RCLCPP_ERROR_THROTTLE( - logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); + logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, + raw_ctrl_cmd.acc); - return Motion{vel, acc}; + return raw_ctrl_cmd; } void PidLongitudinalController::updateControlState(const ControlData & control_data) { const double current_vel = control_data.current_motion.vel; - const double current_acc = control_data.current_motion.acc; const double stop_dist = control_data.stop_dist; // flags for state transition @@ -601,8 +605,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; - const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && - std::abs(current_acc) < p.stopped_state_entry_acc; + const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel; + // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also // considered as a stop const bool is_not_running = [&]() { @@ -703,7 +707,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); // prevent the car from taking a long time to start to move - m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); + m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_raw_ctrl_cmd.acc); return changeState(ControlState::DRIVE); } return; @@ -749,8 +753,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); - // prevent the car from taking a long time to start to move - m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); return changeState(ControlState::DRIVE); } @@ -782,55 +784,85 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const size_t target_idx = control_data.target_idx; // velocity and acceleration command - Motion raw_ctrl_cmd{ + Motion ctrl_cmd_as_pedal_pos{ control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps, control_data.interpolated_traj.points.at(target_idx).acceleration_mps2}; - if (m_control_state == ControlState::DRIVE) { - raw_ctrl_cmd.vel = - control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; - raw_ctrl_cmd.acc = applyVelocityFeedback(control_data); - raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx); + if (m_control_state == ControlState::STOPPED) { + const auto & p = m_stopped_state_params; + ctrl_cmd_as_pedal_pos.vel = p.vel; + ctrl_cmd_as_pedal_pos.acc = p.acc; - RCLCPP_DEBUG( - logger_, - "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " - "feedback_ctrl_cmd.ac: %3.3f", - raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel, - control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps, - raw_ctrl_cmd.acc); - } else if (m_control_state == ControlState::STOPPING) { - raw_ctrl_cmd.acc = m_smooth_stop.calculate( - control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, - m_vel_hist, m_delay_compensation_time); - raw_ctrl_cmd.vel = m_stopped_state_params.vel; + m_prev_raw_ctrl_cmd.vel = 0.0; + m_prev_raw_ctrl_cmd.acc = 0.0; + + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, ctrl_cmd_as_pedal_pos.acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, ctrl_cmd_as_pedal_pos.acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc); RCLCPP_DEBUG( - logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, - raw_ctrl_cmd.acc); - } else if (m_control_state == ControlState::STOPPED) { - // This acceleration is without slope compensation - const auto & p = m_stopped_state_params; - raw_ctrl_cmd.vel = p.vel; - raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter( - p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); + logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", ctrl_cmd_as_pedal_pos.vel, + ctrl_cmd_as_pedal_pos.acc); + } else { + Motion raw_ctrl_cmd{ + control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps, + control_data.interpolated_traj.points.at(target_idx).acceleration_mps2}; + if (m_control_state == ControlState::EMERGENCY) { + raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); + } else { + if (m_control_state == ControlState::DRIVE) { + raw_ctrl_cmd.vel = control_data.interpolated_traj.points.at(control_data.target_idx) + .longitudinal_velocity_mps; + raw_ctrl_cmd.acc = applyVelocityFeedback(control_data); + raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx); + + RCLCPP_DEBUG( + logger_, + "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " + "feedback_ctrl_cmd.ac: %3.3f", + raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel, + control_data.interpolated_traj.points.at(control_data.target_idx) + .longitudinal_velocity_mps, + raw_ctrl_cmd.acc); + } else if (m_control_state == ControlState::STOPPING) { + raw_ctrl_cmd.acc = m_smooth_stop.calculate( + control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, + m_vel_hist, m_delay_compensation_time); + raw_ctrl_cmd.vel = m_stopped_state_params.vel; + + RCLCPP_DEBUG( + logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, + raw_ctrl_cmd.acc); + } + raw_ctrl_cmd.acc = std::clamp(raw_ctrl_cmd.acc, m_min_acc, m_max_acc); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, raw_ctrl_cmd.acc); + raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter( + raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc); + } + + // store acceleration without slope compensation + m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - RCLCPP_DEBUG(logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); - } else if (m_control_state == ControlState::EMERGENCY) { - raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); + ctrl_cmd_as_pedal_pos.acc = + applySlopeCompensation(raw_ctrl_cmd.acc, control_data.slope_angle, control_data.shift); + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc); + ctrl_cmd_as_pedal_pos.vel = raw_ctrl_cmd.vel; } - // store acceleration without slope compensation - m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + storeAccelCmd(m_prev_raw_ctrl_cmd.acc); - // apply slope compensation and filter acceleration and jerk - const double filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data); - const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; + ctrl_cmd_as_pedal_pos.acc = longitudinal_utils::applyDiffLimitFilter( + ctrl_cmd_as_pedal_pos.acc, m_prev_ctrl_cmd.acc, control_data.dt, m_max_acc_cmd_diff); // update debug visualization updateDebugVelAcc(control_data); - return filtered_ctrl_cmd; + RCLCPP_DEBUG( + logger_, "[final output]: acc: %3.3f, v_curr: %3.3f", ctrl_cmd_as_pedal_pos.acc, + control_data.current_motion.vel); + + return ctrl_cmd_as_pedal_pos; } // Do not use nearest_idx here @@ -914,28 +946,6 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift return m_prev_shift; } -double PidLongitudinalController::calcFilteredAcc( - const double raw_acc, const ControlData & control_data) -{ - const double acc_max_filtered = std::clamp(raw_acc, m_min_acc, m_max_acc); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); - - // store ctrl cmd without slope filter - storeAccelCmd(acc_max_filtered); - - // apply slope compensation - const double acc_slope_filtered = - applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); - - // This jerk filter must be applied after slope compensation - const double acc_jerk_filtered = longitudinal_utils::applyDiffLimitFilter( - acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); - - return acc_jerk_filtered; -} - void PidLongitudinalController::storeAccelCmd(const double accel) { if (m_control_state == ControlState::DRIVE) { diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index ad6217663fbae..5027c94afe7c1 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -53,12 +53,11 @@ # stopped state stopped_vel: 0.0 - stopped_acc: -3.4 - stopped_jerk: -5.0 + stopped_acc: -3.4 # denotes pedal position # emergency state emergency_vel: 0.0 - emergency_acc: -5.0 + emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 # acceleration limit @@ -68,6 +67,7 @@ # jerk limit max_jerk: 2.0 min_jerk: -5.0 + max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1] # slope compensation lpf_pitch_gain: 0.95 diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md index 2d6f5c5f949af..e46db3c06cfeb 100644 --- a/control/autoware_vehicle_cmd_gate/README.md +++ b/control/autoware_vehicle_cmd_gate/README.md @@ -78,6 +78,10 @@ The limitation values are calculated based on the 1D interpolation of the limita Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published. +Notation 2: If you use vehicles in which the driving force is controlled by the accelerator/brake pedal, the jerk limit, denoting the pedal rate limit, must be sufficiently relaxed at low speeds. +Otherwise, quick pedal changes at start/stop will not be possible, resulting in slow starts and creep down on hills. +This functionality for starting/stopping was embedded in the source code but was removed because it was complex and could be achieved by parameters. + ## Assumptions / Known limits The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 54c87f45b6a96..74affea696893 100644 --- a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -16,14 +16,14 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [20.0, 30.0] - steer_lim: [1.0, 0.8] - steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.0] - lat_acc_lim: [5.0, 4.0] - lat_jerk_lim: [7.0, 6.0] - actual_steer_diff_lim: [1.0, 0.8] + reference_speed_points: [0.1, 0.3, 20.0, 30.0] + steer_lim: [1.0, 1.0, 1.0, 0.8] + steer_rate_lim: [1.0, 1.0, 1.0, 0.8] + lon_acc_lim: [5.0, 5.0, 5.0, 4.0] + lon_jerk_lim: [80.0, 5.0, 5.0, 4.0] # The first element is required for quick pedal changes when stopping and starting. + lat_acc_lim: [5.0, 5.0, 5.0, 4.0] + lat_jerk_lim: [7.0, 7.0, 7.0, 6.0] + actual_steer_diff_lim: [1.0, 1.0, 1.0, 0.8] on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 87e79f59bc356..dd6e2f0f54aea 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -607,7 +607,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); - const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); @@ -618,23 +617,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in) if (mode.is_in_transition) { filter_on_transition_.filterAll(dt, current_steer_, out, is_filter_activated); } else { - // When ego is stopped and the input command is not stopping, - // use the higher of actual vehicle longitudinal state - // and previous longitudinal command for the filtering - // this is to prevent the jerk limits being applied and causing - // a delay when restarting the vehicle. - - if (ego_is_stopped && !input_cmd_is_stopping) { - auto prev_cmd = filter_.getPrevCmd(); - prev_cmd.longitudinal.acceleration = - std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); - // consider reverse driving - prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > - std::fabs(current_status_cmd.longitudinal.velocity) - ? prev_cmd.longitudinal.velocity - : current_status_cmd.longitudinal.velocity; - filter_.setPrevCmd(prev_cmd); - } filter_.filterAll(dt, current_steer_, out, is_filter_activated); } diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 42e28d633d16b..22da5b0c5de50 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -223,7 +223,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_pub_status_; void onTimer(); - void publishControlCommands(const Commands & input_msg); + void publishControlCommands(const Commands & commands); void publishEmergencyStopControlCommands(); void publishStatus(); @@ -243,7 +243,7 @@ class VehicleCmdGate : public rclcpp::Node Control getActualStatusAsCommand(); VehicleCmdFilter filter_; - Control filterControlCommand(const Control & msg); + Control filterControlCommand(const Control & in); // filtering on transition OperationModeState current_operation_mode_; 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/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index 84e11208770b8..0070c07fe4aa7 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -13,7 +13,7 @@ - + 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/launch/planning_evaluator.launch.xml b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml index c9517404b63a9..ec509993afc18 100644 --- a/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/autoware_planning_evaluator/launch/planning_evaluator.launch.xml @@ -22,7 +22,7 @@ - + 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/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index 9c49605d944b5..0ad0872723d55 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -52,10 +52,10 @@ MarkerArray createFootprintMarkerArray( const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( - const std::vector points, const std::string & ns, const int32_t id, const float r, + const std::vector & points, const std::string & ns, const int32_t id, const float r, const float g, const float b); MarkerArray createPointsMarkerArray( - const std::vector poses, const std::string & ns, const int32_t id, const float r, + const std::vector & poses, const std::string & ns, const int32_t id, const float r, const float g, const float b); MarkerArray createPoseMarkerArray( @@ -63,7 +63,7 @@ MarkerArray createPoseMarkerArray( const float & b); MarkerArray createPosesMarkerArray( - const std::vector poses, std::string && ns, const int32_t & first_id, const float & r, + const std::vector & poses, std::string && ns, const int32_t & first_id, const float & r, const float & g, const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, const float & z_scale = 0.2); @@ -74,7 +74,7 @@ MarkerArray createObjectPolygonMarkerArray( const float & g, const float & b); MarkerArray createDeviationLines( - const std::vector poses1, const std::vector poses2, const std::string & ns, + const std::vector & poses1, const std::vector & poses2, const std::string & ns, const int32_t & first_id, const float r, const float g, const float b); } // namespace marker_utils diff --git a/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml index 2ef179dbe3ff9..fc045da0302b7 100644 --- a/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml +++ b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml @@ -6,8 +6,8 @@ - - + + 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/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index a7a697c4efff6..0c6d1cac4246e 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -76,7 +76,7 @@ MarkerArray createFootprintMarkerArray( } MarkerArray createPointsMarkerArray( - const std::vector points, const std::string & ns, const int32_t id, const float r, + const std::vector & points, const std::string & ns, const int32_t id, const float r, const float g, const float b) { auto marker = createDefaultMarker( @@ -93,7 +93,7 @@ MarkerArray createPointsMarkerArray( } MarkerArray createPointsMarkerArray( - const std::vector poses, const std::string & ns, const int32_t id, const float r, + const std::vector & poses, const std::string & ns, const int32_t id, const float r, const float g, const float b) { auto marker = createDefaultMarker( @@ -110,7 +110,7 @@ MarkerArray createPointsMarkerArray( } MarkerArray createDeviationLines( - const std::vector poses1, const std::vector poses2, const std::string & ns, + const std::vector & poses1, const std::vector & poses2, const std::string & ns, const int32_t & first_id, const float r, const float g, const float b) { MarkerArray msg; @@ -144,7 +144,7 @@ MarkerArray createPoseMarkerArray( } MarkerArray createPosesMarkerArray( - const std::vector poses, std::string && ns, const int32_t & first_id, const float & r, + const std::vector & poses, std::string && ns, const int32_t & first_id, const float & r, const float & g, const float & b, const float & x_scale, const float & y_scale, const float & z_scale) { diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index d8f3ef55b15dd..f5582b7aeb305 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -221,7 +221,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index 63dac42f2063e..7040430d5d1ac 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -9,21 +9,21 @@ - + - + - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index b6f3ad4c27eea..b3dc75bbf79cc 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -19,6 +19,7 @@ automatic_pose_initializer autoware_ar_tag_based_localizer + autoware_pointcloud_preprocessor eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt @@ -26,7 +27,6 @@ geo_pose_projector gyro_odometer ndt_scan_matcher - pointcloud_preprocessor pose_estimator_arbiter pose_initializer pose_instability_detector 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 9256e90ec3e35..9cc5fcf847212 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 @@ -42,7 +42,7 @@ - - + @@ -98,7 +98,7 @@ - + @@ -112,7 +112,7 @@ - + @@ -123,7 +123,7 @@ - + @@ -166,7 +166,7 @@ - + @@ -179,7 +179,7 @@ - + @@ -222,14 +222,14 @@ - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index 98e8ec7c8ed4b..ed2d41e95f793 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -17,10 +17,10 @@ - + - + @@ -37,10 +37,10 @@ - + - + @@ -59,14 +59,14 @@ - + - + @@ -76,7 +76,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index 38ee946201f7e..76ed8ad5b2700 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -10,7 +10,7 @@ - + @@ -21,7 +21,7 @@ - + @@ -29,7 +29,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_filter.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_filter.launch.xml index d7bbcb01472eb..cc6f0dee3454f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_filter.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_filter.launch.xml @@ -9,14 +9,14 @@ - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_validator.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_validator.launch.xml index 9a09ad2871e85..69ef4686434da 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_validator.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/object_validator.launch.xml @@ -10,7 +10,7 @@ - + @@ -18,7 +18,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py index 6c0e5f20b8957..4fda3521c07e9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py @@ -56,8 +56,8 @@ def create_no_compare_map_pipeline(self): components = [] components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -84,8 +84,8 @@ def create_compare_map_pipeline(self): ) components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -105,7 +105,7 @@ def create_compare_map_pipeline(self): ) components.append( ComposableNode( - package="compare_map_segmentation", + package="autoware_compare_map_segmentation", plugin="autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent", name="voxel_based_compare_map_filter", remappings=[ diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml index 151e3f28739d9..80beb9c236f27 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/filter/radar_filter.launch.xml @@ -4,7 +4,7 @@ - + @@ -33,7 +33,7 @@ - + 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..196325974c10c 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 @@ - - + + @@ -75,7 +75,7 @@ - + @@ -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..b14ccc563ebae 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 @@ - - + + @@ -98,7 +98,7 @@ - + @@ -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/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index fd38b7bfd6ab3..541d4e4d4c920 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -91,8 +91,8 @@ def create_additional_pipeline(self, lidar_name): components = [] components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name=f"{lidar_name}_crop_box_filter", remappings=[ ("input", f"/sensing/lidar/{lidar_name}/pointcloud"), @@ -115,7 +115,7 @@ def create_additional_pipeline(self, lidar_name): components.append( ComposableNode( - package="ground_segmentation", + package="autoware_ground_segmentation", plugin="autoware::ground_segmentation::" + ground_segmentation_plugin_name, name=f"{lidar_name}_ground_filter", remappings=[ @@ -137,8 +137,8 @@ def create_ransac_pipeline(self): components = [] components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", namespace="plane_fitting", remappings=[ @@ -161,8 +161,8 @@ def create_ransac_pipeline(self): components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name="short_height_obstacle_detection_area_filter", namespace="plane_fitting", remappings=[ @@ -186,8 +186,8 @@ def create_ransac_pipeline(self): components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::Lanelet2MapFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::Lanelet2MapFilterComponent", name="vector_map_filter", namespace="plane_fitting", remappings=[ @@ -208,7 +208,7 @@ def create_ransac_pipeline(self): components.append( ComposableNode( - package="ground_segmentation", + package="autoware_ground_segmentation", plugin="autoware::ground_segmentation::" + "RANSACGroundFilterComponent", name="ransac_ground_filter", namespace="plane_fitting", @@ -243,8 +243,8 @@ def create_common_pipeline(self, input_topic, output_topic): components = [] components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name="crop_box_filter", remappings=[ ("input", input_topic), @@ -267,7 +267,7 @@ def create_common_pipeline(self, input_topic, output_topic): components.append( ComposableNode( - package="ground_segmentation", + package="autoware_ground_segmentation", plugin="autoware::ground_segmentation::" + ground_segmentation_plugin_name, name="common_ground_filter", remappings=[ @@ -331,7 +331,7 @@ def create_time_series_outlier_filter_components(input_topic, output_topic): components = [] components.append( ComposableNode( - package="occupancy_grid_map_outlier_filter", + package="autoware_occupancy_grid_map_outlier_filter", plugin="autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", name="occupancy_grid_based_outlier_filter", remappings=[ @@ -352,8 +352,8 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con components = [] components.append( ComposableNode( - package="elevation_map_loader", - plugin="ElevationMapLoaderNode", + package="autoware_elevation_map_loader", + plugin="autoware::elevation_map_loader::ElevationMapLoaderNode", name="elevation_map_loader", namespace="elevation_map", remappings=[ @@ -379,7 +379,11 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con ] ), "elevation_map_directory": PathJoinSubstitution( - [FindPackageShare("elevation_map_loader"), "data", "elevation_maps"] + [ + FindPackageShare("autoware_elevation_map_loader"), + "data", + "elevation_maps", + ] ), "use_elevation_map_cloud_publisher": False, } @@ -390,7 +394,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con components.append( ComposableNode( - package="compare_map_segmentation", + package="autoware_compare_map_segmentation", plugin="autoware::compare_map_segmentation::CompareElevationMapFilterComponent", name="compare_elevation_map_filter", namespace="elevation_map", @@ -416,8 +420,8 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", name="voxel_grid_filter", namespace="elevation_map", remappings=[ @@ -441,8 +445,8 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con components.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::VoxelGridOutlierFilterComponent", name="voxel_grid_outlier_filter", namespace="elevation_map", remappings=[ @@ -468,8 +472,8 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con @staticmethod def get_additional_lidars_concatenated_component(input_topics, output_topic): return ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", remappings=[ ("~/input/odom", "/localization/kinematic_state"), @@ -488,8 +492,8 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic): @staticmethod def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, output_topic): return ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_no_ground_data", remappings=[ ("~/input/odom", "/localization/kinematic_state"), diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index f99416cc5ac0a..d92ced54bbc07 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -16,7 +16,7 @@ - + @@ -32,7 +32,7 @@ - + @@ -50,7 +50,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6c428a5adea0b..71c7544f4dd94 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -144,7 +144,12 @@ - + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 6cc47de76ea96..9defa5f9607fa 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -3,10 +3,10 @@ - + - + @@ -24,9 +24,9 @@ - - - + + + @@ -44,7 +44,7 @@ - + @@ -72,7 +72,7 @@ - + @@ -99,7 +99,7 @@ - + @@ -127,7 +127,7 @@ - + @@ -140,7 +140,7 @@ - + @@ -150,7 +150,7 @@ - + @@ -177,6 +177,6 @@ - + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index e467aba5d4e98..74b89e9298da4 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -55,8 +55,8 @@ def create_parameter_dict(*args): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ ComposableNode( - package="traffic_light_classifier", - plugin="traffic_light::TrafficLightClassifierNodelet", + package="autoware_traffic_light_classifier", + plugin="autoware::traffic_light::TrafficLightClassifierNodelet", name="car_traffic_light_classifier", namespace="classification", parameters=[car_traffic_light_classifier_model_param], @@ -70,8 +70,8 @@ def create_parameter_dict(*args): ], ), ComposableNode( - package="traffic_light_classifier", - plugin="traffic_light::TrafficLightClassifierNodelet", + package="autoware_traffic_light_classifier", + plugin="autoware::traffic_light::TrafficLightClassifierNodelet", name="pedestrian_traffic_light_classifier", namespace="classification", parameters=[pedestrian_traffic_light_classifier_model_param], @@ -85,10 +85,10 @@ def create_parameter_dict(*args): ], ), ComposableNode( - package="traffic_light_visualization", - plugin="traffic_light::TrafficLightRoiVisualizerNodelet", + package="autoware_traffic_light_visualization", + plugin="autoware::traffic_light::TrafficLightRoiVisualizerNode", name="traffic_light_roi_visualizer", - parameters=[create_parameter_dict("enable_fine_detection")], + parameters=[create_parameter_dict("enable_fine_detection", "use_image_transport")], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), @@ -113,8 +113,8 @@ def create_parameter_dict(*args): decompressor_loader = LoadComposableNodes( composable_node_descriptions=[ ComposableNode( - package="image_transport_decompressor", - plugin="image_preprocessor::ImageTransportDecompressor", + package="autoware_image_transport_decompressor", + plugin="autoware::image_preprocessor::ImageTransportDecompressor", name="traffic_light_image_decompressor", parameters=[{"encoding": "rgb8"}], remappings=[ @@ -136,8 +136,8 @@ def create_parameter_dict(*args): fine_detector_loader = LoadComposableNodes( composable_node_descriptions=[ ComposableNode( - package="traffic_light_fine_detector", - plugin="traffic_light::TrafficLightFineDetectorNodelet", + package="autoware_traffic_light_fine_detector", + plugin="autoware::traffic_light::TrafficLightFineDetectorNode", name="traffic_light_fine_detector", namespace="detection", parameters=[fine_detector_model_param], @@ -168,10 +168,11 @@ def add_launch_arg(name: str, default_value=None, description=None): DeclareLaunchArgument(name, default_value=default_value, description=description) ) - fine_detector_share_dir = get_package_share_directory("traffic_light_fine_detector") - classifier_share_dir = get_package_share_directory("traffic_light_classifier") + fine_detector_share_dir = get_package_share_directory("autoware_traffic_light_fine_detector") + classifier_share_dir = get_package_share_directory("autoware_traffic_light_classifier") add_launch_arg("enable_image_decompressor", "True") add_launch_arg("enable_fine_detection", "True") + add_launch_arg("use_image_transport", "True") add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") add_launch_arg( diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index db2968f735a37..40071c39f1e75 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -14,41 +14,41 @@ autoware_cmake autoware_cluster_merger + autoware_compare_map_segmentation autoware_crosswalk_traffic_light_estimator + autoware_detected_object_feature_remover + autoware_detected_object_validation autoware_detection_by_tracker + autoware_elevation_map_loader + autoware_euclidean_cluster + autoware_ground_segmentation + autoware_image_projection_based_fusion + autoware_image_transport_decompressor + autoware_lidar_apollo_instance_segmentation autoware_map_based_prediction + autoware_multi_object_tracker + autoware_object_merger autoware_object_range_splitter autoware_object_velocity_splitter + autoware_occupancy_grid_map_outlier_filter + autoware_pointcloud_preprocessor + autoware_probabilistic_occupancy_grid_map autoware_radar_crossing_objects_noise_filter autoware_radar_fusion_to_detected_object autoware_radar_object_clustering autoware_radar_object_tracker autoware_raindrop_cluster_filter - compare_map_segmentation - detected_object_feature_remover - detected_object_validation - 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 + autoware_shape_estimation + autoware_tracking_object_merger + autoware_traffic_light_arbiter + autoware_traffic_light_classifier + autoware_traffic_light_fine_detector + autoware_traffic_light_map_based_detector + autoware_traffic_light_multi_camera_fusion + autoware_traffic_light_occlusion_predictor + autoware_traffic_light_visualization pointcloud_to_laserscan - probabilistic_occupancy_grid_map - shape_estimation topic_tools - tracking_object_merger - traffic_light_arbiter - traffic_light_classifier - traffic_light_fine_detector - traffic_light_map_based_detector - traffic_light_multi_camera_fusion - traffic_light_occlusion_predictor - traffic_light_visualization ament_lint_auto autoware_lint_common diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 3c58c8d85b5be..d88601497096d 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -261,7 +261,7 @@ - + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index bf43fde85e0a2..8d24e6ef6b294 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -13,6 +13,7 @@ + @@ -105,7 +106,7 @@ - + @@ -113,7 +114,7 @@ - + @@ -148,7 +149,7 @@ - + @@ -159,6 +160,7 @@ + diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml index 1b2469d108dfb..e8597a6fc9a4e 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml @@ -16,10 +16,9 @@ ament_cmake autoware_cmake - ament_index_cpp + ament_index_cpp aruco autoware_landmark_manager - autoware_lanelet2_extension cv_bridge diagnostic_msgs localization_util diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index cef3debf22a4f..80845402a59f3 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -116,7 +116,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) */ using std::placeholders::_1; map_bin_sub_ = this->create_subscription( - "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + "~/input/lanelet2_map", rclcpp::QoS(1).durability(rclcpp::DurabilityPolicy::TransientLocal), std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); @@ -131,7 +131,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) /* Publishers */ - const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); + const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); const rclcpp::QoS qos_pub_periodic(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); pose_pub_ = this->create_publisher( "~/output/pose_with_covariance", qos_pub_periodic); diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml index 4880cf88bec5d..d69adc7904411 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -20,6 +20,7 @@ autoware_lanelet2_extension autoware_map_msgs + autoware_universe_utils geometry_msgs localization_util rclcpp diff --git a/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp b/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp index ffafe41269468..3a253d785e8a7 100644 --- a/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp +++ b/localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp @@ -65,7 +65,7 @@ class PoseCovarianceModifierNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_double_gnss_position_stddev_; void callback_gnss_pose_with_cov( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg); + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in); void callback_ndt_pose_with_cov( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in); diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 02602cbebff37..c4fa77ce85a6b 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -21,7 +21,6 @@ autoware_universe_utils diagnostic_msgs - diagnostic_updater localization_util nav_msgs rclcpp_components 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/localization_util/package.xml b/localization/localization_util/package.xml index bb1ca3123ad27..86c74bb92f2f8 100644 --- a/localization/localization_util/package.xml +++ b/localization/localization_util/package.xml @@ -17,21 +17,13 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils - fmt + diagnostic_msgs geometry_msgs - nav_msgs rclcpp - sensor_msgs std_msgs - std_srvs tf2 tf2_eigen tf2_geometry_msgs - tf2_ros - tf2_sensor_msgs - tier4_debug_msgs - tier4_localization_msgs visualization_msgs ament_cmake_cppcheck 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..5d6b8d73440e4 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -117,7 +117,10 @@ initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] # In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance - temperature: 0.1 + temperature: 0.05 + + # Scale value for adjusting the estimated covariance by a constant multiplication + scale_factor: 1.0 dynamic_map_loading: 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 e09ff5c782e7d..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" 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/package.xml b/localization/ndt_scan_matcher/package.xml index 932ee55874cea..c88760d3db92c 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -30,7 +30,6 @@ rclcpp rclcpp_components sensor_msgs - std_msgs std_srvs tf2 tf2_eigen 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..a60277f90fa64 100644 --- a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -25,7 +25,13 @@ "temperature": { "type": "number", "description": "In MULTI_NDT_SCORE, the parameter that adjusts the estimated 2D covariance", - "default": 0.1, + "default": 0.05, + "exclusiveMinimum": 0 + }, + "scale_factor": { + "type": "number", + "description": "Scale value for adjusting the estimated covariance by a constant multiplication", + "default": 1.0, "exclusiveMinimum": 0 } }, @@ -33,7 +39,9 @@ "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/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index b394a8f863ceb..d64107b03ebba 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -51,18 +51,6 @@ tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( return tier4_debug_msgs::build().stamp(stamp).data(data); } -Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( - const Eigen::Matrix2d & matrix) -{ - const Eigen::SelfAdjointEigenSolver eigensolver(matrix); - if (eigensolver.info() == Eigen::Success) { - const Eigen::Vector2d eigen_vec = eigensolver.eigenvectors().col(0); - const double th = std::atan2(eigen_vec.y(), eigen_vec.x()); - return Eigen::Rotation2Dd(th).toRotationMatrix(); - } - throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); -} - std::array rotate_covariance( const std::array & src_covariance, const Eigen::Matrix3d & rotation) { @@ -584,8 +572,12 @@ 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 double default_cov_xx = param_.covariance.output_pose_covariance[0]; + const double default_cov_yy = param_.covariance.output_pose_covariance[7]; + const Eigen::Matrix2d estimated_covariance_2d_adj = pclomp::adjust_diagonal_covariance( + estimated_covariance_2d_scaled, ndt_result.pose, default_cov_xx, default_cov_yy); 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); @@ -830,17 +822,6 @@ Eigen::Matrix2d NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) { - Eigen::Matrix2d rot = Eigen::Matrix2d::Identity(); - try { - rot = find_rotation_matrix_aligning_covariance_to_principal_axes( - ndt_result.hessian.inverse().block(0, 0, 2, 2)); - } catch (const std::exception & e) { - std::stringstream message; - message << "Error in Eigen solver: " << e.what(); - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - return Eigen::Matrix2d::Identity() * param_.covariance.output_pose_covariance[0 + 6 * 0]; - } - geometry_msgs::msg::PoseArray multi_ndt_result_msg; geometry_msgs::msg::PoseArray multi_initial_pose_msg; multi_ndt_result_msg.header.stamp = sensor_ros_time; diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 1c1c926c75b7a..d97c9f15fc8ae 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -25,15 +25,11 @@ geometry_msgs magic_enum pcl_conversions - pcl_ros - pluginlib rclcpp_components sensor_msgs std_msgs std_srvs - ublox_msgs visualization_msgs - yabloc_particle_filter ament_cmake_ros ament_lint_auto diff --git a/localization/pose_initializer/CMakeLists.txt b/localization/pose_initializer/CMakeLists.txt index 08d5bb47fca09..324488bedebfc 100644 --- a/localization/pose_initializer/CMakeLists.txt +++ b/localization/pose_initializer/CMakeLists.txt @@ -7,8 +7,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_initializer/pose_initializer_core.cpp src/pose_initializer/gnss_module.cpp - src/pose_initializer/ndt_module.cpp - src/pose_initializer/yabloc_module.cpp + src/pose_initializer/localization_module.cpp src/pose_initializer/stop_check_module.cpp src/pose_initializer/ekf_localization_trigger_module.cpp src/pose_initializer/ndt_localization_trigger_module.cpp diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/localization_module.cpp similarity index 64% rename from localization/pose_initializer/src/pose_initializer/ndt_module.cpp rename to localization/pose_initializer/src/pose_initializer/localization_module.cpp index 1f657545e687c..433833cd15784 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/localization_module.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 The Autoware Contributors +// Copyright 2024 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,38 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_module.hpp" +#include "localization_module.hpp" #include #include #include +#include using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -NdtModule::NdtModule(rclcpp::Node * node) -: logger_(node->get_logger()), cli_align_(node->create_client("ndt_align")) +LocalizationModule::LocalizationModule(rclcpp::Node * node, const std::string & service_name) +: logger_(node->get_logger()) { + cli_align_ = node->create_client(service_name); } -PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped & pose) +PoseWithCovarianceStamped LocalizationModule::align_pose(const PoseWithCovarianceStamped & pose) { const auto req = std::make_shared(); req->pose_with_covariance = pose; if (!cli_align_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("NDT align server is not ready."); + throw component_interface_utils::ServiceUnready("align server is not ready."); } - RCLCPP_INFO(logger_, "Call NDT align server."); + RCLCPP_INFO(logger_, "Call align server."); const auto res = cli_align_->async_send_request(req).get(); if (!res->success) { - throw ServiceException( - Initialize::Service::Response::ERROR_ESTIMATION, "NDT align server failed."); + throw ServiceException(Initialize::Service::Response::ERROR_ESTIMATION, "align server failed."); } - RCLCPP_INFO(logger_, "NDT align server succeeded."); + RCLCPP_INFO(logger_, "align server succeeded."); // Overwrite the covariance. return res->pose_with_covariance; diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.hpp b/localization/pose_initializer/src/pose_initializer/localization_module.hpp similarity index 77% rename from localization/pose_initializer/src/pose_initializer/ndt_module.hpp rename to localization/pose_initializer/src/pose_initializer/localization_module.hpp index a6cdea9879e34..f17f68670306c 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/localization_module.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 The Autoware Contributors +// Copyright 2024 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,22 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_INITIALIZER__NDT_MODULE_HPP_ -#define POSE_INITIALIZER__NDT_MODULE_HPP_ +#ifndef POSE_INITIALIZER__LOCALIZATION_MODULE_HPP_ +#define POSE_INITIALIZER__LOCALIZATION_MODULE_HPP_ #include #include #include -class NdtModule +#include + +class LocalizationModule { private: using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; public: - explicit NdtModule(rclcpp::Node * node); + LocalizationModule(rclcpp::Node * node, const std::string & service_name); PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose); private: @@ -35,4 +37,4 @@ class NdtModule rclcpp::Client::SharedPtr cli_align_; }; -#endif // POSE_INITIALIZER__NDT_MODULE_HPP_ +#endif // POSE_INITIALIZER__LOCALIZATION_MODULE_HPP_ diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 7d75a1fc80f2e..6e023a1309c2d 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -17,10 +17,9 @@ #include "copy_vector_to_array.hpp" #include "ekf_localization_trigger_module.hpp" #include "gnss_module.hpp" +#include "localization_module.hpp" #include "ndt_localization_trigger_module.hpp" -#include "ndt_module.hpp" #include "stop_check_module.hpp" -#include "yabloc_module.hpp" #include #include @@ -45,10 +44,10 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) gnss_ = std::make_unique(this); } if (declare_parameter("yabloc_enabled")) { - yabloc_ = std::make_unique(this); + yabloc_ = std::make_unique(this, "yabloc_align"); } if (declare_parameter("ndt_enabled")) { - ndt_ = std::make_unique(this); + ndt_ = std::make_unique(this, "ndt_align"); ndt_localization_trigger_ = std::make_unique(this); } if (declare_parameter("stop_check_enabled")) { diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index a813ec6459810..0a9a0614f6d20 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -25,9 +25,8 @@ #include class StopCheckModule; -class NdtModule; +class LocalizationModule; class GnssModule; -class YabLocModule; class EkfLocalizationTriggerModule; class NdtLocalizationTriggerModule; @@ -50,8 +49,8 @@ class PoseInitializer : public rclcpp::Node std::array output_pose_covariance_{}; std::array gnss_particle_covariance_{}; std::unique_ptr gnss_; - std::unique_ptr ndt_; - std::unique_ptr yabloc_; + std::unique_ptr ndt_; + std::unique_ptr yabloc_; std::unique_ptr stop_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; diff --git a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp deleted file mode 100644 index 204f2289adbf4..0000000000000 --- a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_module.hpp" - -#include -#include - -#include - -using ServiceException = component_interface_utils::ServiceException; -using Initialize = localization_interface::Initialize; -using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - -YabLocModule::YabLocModule(rclcpp::Node * node) -: logger_(node->get_logger()), cli_align_(node->create_client("yabloc_align")) -{ -} - -PoseWithCovarianceStamped YabLocModule::align_pose(const PoseWithCovarianceStamped & pose) -{ - const auto req = std::make_shared(); - req->pose_with_covariance = pose; - - if (!cli_align_->service_is_ready()) { - throw component_interface_utils::ServiceUnready("YabLoc align server is not ready."); - } - - RCLCPP_INFO(logger_, "Call YabLoc align server."); - const auto res = cli_align_->async_send_request(req).get(); - if (!res->success) { - RCLCPP_INFO(logger_, "YabLoc align server failed."); - throw ServiceException( - Initialize::Service::Response::ERROR_ESTIMATION, "YabLoc align server failed."); - } - RCLCPP_INFO(logger_, "YabLoc align server succeeded."); - - // Overwrite the covariance. - return res->pose_with_covariance; -} diff --git a/localization/pose_initializer/src/pose_initializer/yabloc_module.hpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.hpp deleted file mode 100644 index 4c6076553b059..0000000000000 --- a/localization/pose_initializer/src/pose_initializer/yabloc_module.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POSE_INITIALIZER__YABLOC_MODULE_HPP_ -#define POSE_INITIALIZER__YABLOC_MODULE_HPP_ - -#include - -#include -#include - -class YabLocModule -{ -private: - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; - -public: - explicit YabLocModule(rclcpp::Node * node); - PoseWithCovarianceStamped align_pose(const PoseWithCovarianceStamped & pose); - -private: - rclcpp::Logger logger_; - rclcpp::Client::SharedPtr cli_align_; -}; - -#endif // POSE_INITIALIZER__YABLOC_MODULE_HPP_ diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index 04e9747d3723b..7e0237cbaf780 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -17,7 +17,9 @@ ament_cmake_auto autoware_cmake - ament_index_cpp + ament_cmake_cppcheck + ament_index_cpp + ament_lint_auto autoware_universe_utils diagnostic_msgs geometry_msgs @@ -26,10 +28,6 @@ rclcpp_components tf2 tf2_geometry_msgs - tier4_debug_msgs - - ament_cmake_cppcheck - ament_lint_auto ament_cmake diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp index 52752bff65770..d96285c0d5c00 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -40,7 +40,7 @@ GroundServer::GroundServer(const rclcpp::NodeOptions & options) { using std::placeholders::_1; using std::placeholders::_2; - const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable(); + const rclcpp::QoS map_qos = rclcpp::QoS(1).transient_local().reliable(); auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1); auto on_map = std::bind(&GroundServer::on_map, this, _1); diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index c95431d128000..6c3d43f33440f 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -27,8 +27,8 @@ namespace yabloc::ll2_decomposer Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options) { using std::placeholders::_1; - const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local(); - const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable(); + const rclcpp::QoS latch_qos = rclcpp::QoS(1).transient_local(); + const rclcpp::QoS map_qos = rclcpp::QoS(1).transient_local().reliable(); // Publisher pub_road_marking_ = create_publisher("~/output/ll2_road_marking", latch_qos); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 73b0164f0c8a7..92f7668c4d774 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -25,7 +25,7 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. 3. **The division size along each axis should be equal.** -4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [autoware_compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_compare_map_segmentation). 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. diff --git a/perception/bytetrack/CMakeLists.txt b/perception/autoware_bytetrack/CMakeLists.txt similarity index 80% rename from perception/bytetrack/CMakeLists.txt rename to perception/autoware_bytetrack/CMakeLists.txt index c020843af56b5..227e1fe4a1b13 100644 --- a/perception/bytetrack/CMakeLists.txt +++ b/perception/autoware_bytetrack/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(bytetrack) +project(autoware_bytetrack) find_package(autoware_cmake REQUIRED) autoware_package() @@ -16,20 +16,20 @@ find_package(Boost REQUIRED) # # Core library # -file(GLOB bytetrack_lib_src +file(GLOB project_lib_src "lib/src/*.cpp" ) -ament_auto_add_library(bytetrack_lib SHARED - ${bytetrack_lib_src} +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + ${project_lib_src} ) -target_include_directories(bytetrack_lib +target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ ) -target_link_libraries(bytetrack_lib +target_link_libraries(${PROJECT_NAME}_lib Eigen3::Eigen yaml-cpp ) @@ -72,8 +72,8 @@ target_compile_definitions(${PROJECT_NAME}_node PRIVATE ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "bytetrack::ByteTrackNode" - EXECUTABLE ${PROJECT_NAME}_node_exe + PLUGIN "autoware::bytetrack::ByteTrackNode" + EXECUTABLE bytetrack_node_exe ) # @@ -88,8 +88,8 @@ ament_target_dependencies(${PROJECT_NAME}_visualizer ) rclcpp_components_register_node(${PROJECT_NAME}_visualizer - PLUGIN "bytetrack::ByteTrackVisualizerNode" - EXECUTABLE ${PROJECT_NAME}_visualizer_node_exe + PLUGIN "autoware::bytetrack::ByteTrackVisualizerNode" + EXECUTABLE bytetrack_visualizer_node_exe ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/bytetrack/README.md b/perception/autoware_bytetrack/README.md similarity index 100% rename from perception/bytetrack/README.md rename to perception/autoware_bytetrack/README.md diff --git a/perception/bytetrack/config/bytetrack.param.yaml b/perception/autoware_bytetrack/config/bytetrack.param.yaml similarity index 100% rename from perception/bytetrack/config/bytetrack.param.yaml rename to perception/autoware_bytetrack/config/bytetrack.param.yaml diff --git a/perception/bytetrack/config/bytetrack_visualizer.param.yaml b/perception/autoware_bytetrack/config/bytetrack_visualizer.param.yaml similarity index 100% rename from perception/bytetrack/config/bytetrack_visualizer.param.yaml rename to perception/autoware_bytetrack/config/bytetrack_visualizer.param.yaml diff --git a/perception/bytetrack/config/kalman_filter.param.yaml b/perception/autoware_bytetrack/config/kalman_filter.param.yaml similarity index 100% rename from perception/bytetrack/config/kalman_filter.param.yaml rename to perception/autoware_bytetrack/config/kalman_filter.param.yaml diff --git a/perception/bytetrack/include/bytetrack/bytetrack.hpp b/perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack.hpp similarity index 86% rename from perception/bytetrack/include/bytetrack/bytetrack.hpp rename to perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack.hpp index 549ecaac84567..d047619e25a53 100644 --- a/perception/bytetrack/include/bytetrack/bytetrack.hpp +++ b/perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BYTETRACK__BYTETRACK_HPP_ -#define BYTETRACK__BYTETRACK_HPP_ +#ifndef AUTOWARE__BYTETRACK__BYTETRACK_HPP_ +#define AUTOWARE__BYTETRACK__BYTETRACK_HPP_ #include "byte_tracker.h" #include "strack.h" @@ -28,7 +28,7 @@ #include #include -namespace bytetrack +namespace autoware::bytetrack { struct Object { @@ -57,6 +57,6 @@ class ByteTrack ObjectArray latest_objects_; }; -} // namespace bytetrack +} // namespace autoware::bytetrack -#endif // BYTETRACK__BYTETRACK_HPP_ +#endif // AUTOWARE__BYTETRACK__BYTETRACK_HPP_ diff --git a/perception/bytetrack/include/bytetrack/bytetrack_node.hpp b/perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack_node.hpp similarity index 83% rename from perception/bytetrack/include/bytetrack/bytetrack_node.hpp rename to perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack_node.hpp index bd190cb8005d1..7aa52794e0f2f 100644 --- a/perception/bytetrack/include/bytetrack/bytetrack_node.hpp +++ b/perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BYTETRACK__BYTETRACK_NODE_HPP_ -#define BYTETRACK__BYTETRACK_NODE_HPP_ +#ifndef AUTOWARE__BYTETRACK__BYTETRACK_NODE_HPP_ +#define AUTOWARE__BYTETRACK__BYTETRACK_NODE_HPP_ -#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -namespace bytetrack +namespace autoware::bytetrack { using LabelMap = std::map; @@ -55,9 +55,9 @@ class ByteTrackNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - std::unique_ptr bytetrack_; + std::unique_ptr bytetrack_; }; -} // namespace bytetrack +} // namespace autoware::bytetrack -#endif // BYTETRACK__BYTETRACK_NODE_HPP_ +#endif // AUTOWARE__BYTETRACK__BYTETRACK_NODE_HPP_ diff --git a/perception/bytetrack/include/bytetrack/bytetrack_visualizer_node.hpp b/perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack_visualizer_node.hpp similarity index 92% rename from perception/bytetrack/include/bytetrack/bytetrack_visualizer_node.hpp rename to perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack_visualizer_node.hpp index 56f060ecfa9d5..c60c2cb2fcaa5 100644 --- a/perception/bytetrack/include/bytetrack/bytetrack_visualizer_node.hpp +++ b/perception/autoware_bytetrack/include/autoware/bytetrack/bytetrack_visualizer_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ -#define BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ +#ifndef AUTOWARE__BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ +#define AUTOWARE__BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ #include #include @@ -35,7 +35,7 @@ #include #include -namespace bytetrack +namespace autoware::bytetrack { // A helper class to generate bright color instance class ColorMapper @@ -99,6 +99,6 @@ class ByteTrackVisualizerNode : public rclcpp::Node bool use_raw_; ColorMapper color_map_; }; -} // namespace bytetrack +} // namespace autoware::bytetrack -#endif // BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ +#endif // AUTOWARE__BYTETRACK__BYTETRACK_VISUALIZER_NODE_HPP_ diff --git a/perception/bytetrack/launch/bytetrack.launch.xml b/perception/autoware_bytetrack/launch/bytetrack.launch.xml similarity index 72% rename from perception/bytetrack/launch/bytetrack.launch.xml rename to perception/autoware_bytetrack/launch/bytetrack.launch.xml index 3a5a369637453..ace84d4799a8a 100644 --- a/perception/bytetrack/launch/bytetrack.launch.xml +++ b/perception/autoware_bytetrack/launch/bytetrack.launch.xml @@ -4,18 +4,18 @@ - - + + - + - + diff --git a/perception/bytetrack/lib/include/byte_tracker.h b/perception/autoware_bytetrack/lib/include/byte_tracker.h similarity index 100% rename from perception/bytetrack/lib/include/byte_tracker.h rename to perception/autoware_bytetrack/lib/include/byte_tracker.h diff --git a/perception/bytetrack/lib/include/data_type.h b/perception/autoware_bytetrack/lib/include/data_type.h similarity index 100% rename from perception/bytetrack/lib/include/data_type.h rename to perception/autoware_bytetrack/lib/include/data_type.h diff --git a/perception/bytetrack/lib/include/lapjv.h b/perception/autoware_bytetrack/lib/include/lapjv.h similarity index 100% rename from perception/bytetrack/lib/include/lapjv.h rename to perception/autoware_bytetrack/lib/include/lapjv.h diff --git a/perception/bytetrack/lib/include/strack.h b/perception/autoware_bytetrack/lib/include/strack.h similarity index 100% rename from perception/bytetrack/lib/include/strack.h rename to perception/autoware_bytetrack/lib/include/strack.h diff --git a/perception/bytetrack/lib/src/byte_tracker.cpp b/perception/autoware_bytetrack/lib/src/byte_tracker.cpp similarity index 100% rename from perception/bytetrack/lib/src/byte_tracker.cpp rename to perception/autoware_bytetrack/lib/src/byte_tracker.cpp diff --git a/perception/bytetrack/lib/src/lapjv.cpp b/perception/autoware_bytetrack/lib/src/lapjv.cpp similarity index 100% rename from perception/bytetrack/lib/src/lapjv.cpp rename to perception/autoware_bytetrack/lib/src/lapjv.cpp diff --git a/perception/bytetrack/lib/src/strack.cpp b/perception/autoware_bytetrack/lib/src/strack.cpp similarity index 99% rename from perception/bytetrack/lib/src/strack.cpp rename to perception/autoware_bytetrack/lib/src/strack.cpp index 81ba438faee44..4f1e03a4c1d4a 100644 --- a/perception/bytetrack/lib/src/strack.cpp +++ b/perception/autoware_bytetrack/lib/src/strack.cpp @@ -70,7 +70,7 @@ STrack::STrack(std::vector input_tlwh, float score, int label) // load static kf parameters: initialized once in program const std::string package_share_directory = - ament_index_cpp::get_package_share_directory("bytetrack"); + ament_index_cpp::get_package_share_directory("autoware_bytetrack"); const std::string default_config_path = package_share_directory + "/config/kalman_filter.param.yaml"; if (!_parameters_loaded) { diff --git a/perception/bytetrack/lib/src/utils.cpp b/perception/autoware_bytetrack/lib/src/utils.cpp similarity index 100% rename from perception/bytetrack/lib/src/utils.cpp rename to perception/autoware_bytetrack/lib/src/utils.cpp diff --git a/perception/bytetrack/package.xml b/perception/autoware_bytetrack/package.xml similarity index 97% rename from perception/bytetrack/package.xml rename to perception/autoware_bytetrack/package.xml index 231a1845bb0ea..ef2d96e7930d4 100644 --- a/perception/bytetrack/package.xml +++ b/perception/autoware_bytetrack/package.xml @@ -1,7 +1,7 @@ - bytetrack + autoware_bytetrack 0.0.1 ByteTrack implementation ported toward Autoware Manato HIRABAYASHI diff --git a/perception/bytetrack/src/bytetrack.cpp b/perception/autoware_bytetrack/src/bytetrack.cpp similarity index 94% rename from perception/bytetrack/src/bytetrack.cpp rename to perception/autoware_bytetrack/src/bytetrack.cpp index e02be6029ffaf..0edeb0ac4e247 100644 --- a/perception/bytetrack/src/bytetrack.cpp +++ b/perception/autoware_bytetrack/src/bytetrack.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include -namespace bytetrack +namespace autoware::bytetrack { ByteTrack::ByteTrack(const int track_buffer_length) { @@ -66,4 +66,4 @@ ObjectArray ByteTrack::update_tracker(ObjectArray & input_objects) do_inference(input_objects); return latest_objects_; } -} // namespace bytetrack +} // namespace autoware::bytetrack diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/autoware_bytetrack/src/bytetrack_node.cpp similarity index 91% rename from perception/bytetrack/src/bytetrack_node.cpp rename to perception/autoware_bytetrack/src/bytetrack_node.cpp index 64adc83a51abd..1da7ebc4055f0 100644 --- a/perception/bytetrack/src/bytetrack_node.cpp +++ b/perception/autoware_bytetrack/src/bytetrack_node.cpp @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "bytetrack/bytetrack.hpp" - -#include +#include +#include #include #include "autoware_perception_msgs/msg/object_classification.hpp" @@ -24,7 +23,7 @@ #include #include -namespace bytetrack +namespace autoware::bytetrack { ByteTrackNode::ByteTrackNode(const rclcpp::NodeOptions & node_options) : Node("bytetrack", node_options) @@ -34,7 +33,7 @@ ByteTrackNode::ByteTrackNode(const rclcpp::NodeOptions & node_options) int track_buffer_length = declare_parameter("track_buffer_length", 30); - this->bytetrack_ = std::make_unique(track_buffer_length); + this->bytetrack_ = std::make_unique(track_buffer_length); timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ByteTrackNode::on_connect, this)); @@ -80,7 +79,7 @@ void ByteTrackNode::on_rect( object_array.emplace_back(obj); } - bytetrack::ObjectArray objects = bytetrack_->update_tracker(object_array); + autoware::bytetrack::ObjectArray objects = bytetrack_->update_tracker(object_array); for (const auto & tracked_object : objects) { tier4_perception_msgs::msg::DetectedObjectWithFeature object; // fit xy offset to 0 if roi is outside of image @@ -115,7 +114,7 @@ void ByteTrackNode::on_rect( out_objects_uuid.header = msg->header; objects_uuid_pub_->publish(out_objects_uuid); } -} // namespace bytetrack +} // namespace autoware::bytetrack #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(bytetrack::ByteTrackNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::bytetrack::ByteTrackNode) diff --git a/perception/bytetrack/src/bytetrack_visualizer_node.cpp b/perception/autoware_bytetrack/src/bytetrack_visualizer_node.cpp similarity index 97% rename from perception/bytetrack/src/bytetrack_visualizer_node.cpp rename to perception/autoware_bytetrack/src/bytetrack_visualizer_node.cpp index cdb767568a510..4276b86f5cab7 100644 --- a/perception/bytetrack/src/bytetrack_visualizer_node.cpp +++ b/perception/autoware_bytetrack/src/bytetrack_visualizer_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "bytetrack/bytetrack_visualizer_node.hpp" +#include "autoware/bytetrack/bytetrack_visualizer_node.hpp" #include #include @@ -26,7 +26,7 @@ #include #include -namespace bytetrack +namespace autoware::bytetrack { ByteTrackVisualizerNode::ByteTrackVisualizerNode(const rclcpp::NodeOptions & node_options) @@ -190,7 +190,7 @@ void ByteTrackVisualizerNode::draw( } } -} // namespace bytetrack +} // namespace autoware::bytetrack #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(bytetrack::ByteTrackVisualizerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::bytetrack::ByteTrackVisualizerNode) diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/autoware_compare_map_segmentation/CMakeLists.txt similarity index 95% rename from perception/compare_map_segmentation/CMakeLists.txt rename to perception/autoware_compare_map_segmentation/CMakeLists.txt index 0a33711e946ef..664b3f3d8066e 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/autoware_compare_map_segmentation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(compare_map_segmentation) +project(autoware_compare_map_segmentation) find_package(autoware_cmake REQUIRED) autoware_package() @@ -31,7 +31,7 @@ add_library(${PROJECT_NAME} SHARED ) target_link_libraries(${PROJECT_NAME} - pointcloud_preprocessor::pointcloud_preprocessor_filter_base + autoware_pointcloud_preprocessor::pointcloud_preprocessor_filter_base ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} diff --git a/perception/compare_map_segmentation/README.md b/perception/autoware_compare_map_segmentation/README.md similarity index 96% rename from perception/compare_map_segmentation/README.md rename to perception/autoware_compare_map_segmentation/README.md index 0566b5af806db..e775af30239bb 100644 --- a/perception/compare_map_segmentation/README.md +++ b/perception/autoware_compare_map_segmentation/README.md @@ -1,8 +1,8 @@ -# compare_map_segmentation +# autoware_compare_map_segmentation ## Purpose -The `compare_map_segmentation` is a node that filters the ground points from the input pointcloud by using map info (e.g. pcd, elevation map or split map pointcloud from map_loader interface). +The `autoware_compare_map_segmentation` is a package that filters the ground points from the input pointcloud by using map info (e.g. pcd, elevation map or split map pointcloud from map_loader interface). ## Inner-workings / Algorithms diff --git a/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/compare_elevation_map_filter.param.yaml similarity index 100% rename from perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml rename to perception/autoware_compare_map_segmentation/config/compare_elevation_map_filter.param.yaml diff --git a/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml similarity index 100% rename from perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml rename to perception/autoware_compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml diff --git a/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml similarity index 100% rename from perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml rename to perception/autoware_compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml diff --git a/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml similarity index 100% rename from perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml rename to perception/autoware_compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml diff --git a/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/perception/autoware_compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml similarity index 100% rename from perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml rename to perception/autoware_compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml diff --git a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml b/perception/autoware_compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml similarity index 71% rename from perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml rename to perception/autoware_compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml index 9cb41c8780f13..10e506323f15d 100644 --- a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml +++ b/perception/autoware_compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml b/perception/autoware_compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml similarity index 67% rename from perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml rename to perception/autoware_compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml index cdf17ebfeda9f..7169912869968 100644 --- a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml +++ b/perception/autoware_compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml b/perception/autoware_compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml similarity index 65% rename from perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml rename to perception/autoware_compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml index e3f4594729c2b..d291bbd7eadb4 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml +++ b/perception/autoware_compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/autoware_compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml similarity index 76% rename from perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml rename to perception/autoware_compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml index 01ad5cd9b1792..a531b1fb2ec28 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml +++ b/perception/autoware_compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml @@ -1,13 +1,13 @@ - + - + diff --git a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml b/perception/autoware_compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml similarity index 65% rename from perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml rename to perception/autoware_compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml index 13023a8c1c2c9..fd15d0ba21f74 100644 --- a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml +++ b/perception/autoware_compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/perception/compare_map_segmentation/media/compare_elevation_map.png b/perception/autoware_compare_map_segmentation/media/compare_elevation_map.png similarity index 100% rename from perception/compare_map_segmentation/media/compare_elevation_map.png rename to perception/autoware_compare_map_segmentation/media/compare_elevation_map.png diff --git a/perception/compare_map_segmentation/package.xml b/perception/autoware_compare_map_segmentation/package.xml similarity index 86% rename from perception/compare_map_segmentation/package.xml rename to perception/autoware_compare_map_segmentation/package.xml index 7403dc0713475..32d1039f36f4e 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/autoware_compare_map_segmentation/package.xml @@ -1,9 +1,9 @@ - compare_map_segmentation + autoware_compare_map_segmentation 0.1.0 - The ROS 2 compare_map_segmentation package + The ROS 2 autoware_compare_map_segmentation package amc-nu Yukihiro Saito Dai Nguyen @@ -18,12 +18,12 @@ autoware_cmake autoware_map_msgs + autoware_pointcloud_preprocessor autoware_universe_utils grid_map_pcl grid_map_ros nav_msgs pcl_conversions - pointcloud_preprocessor rclcpp rclcpp_components sensor_msgs diff --git a/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/compare_elevation_map_filter.schema.json similarity index 100% rename from perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json rename to perception/autoware_compare_map_segmentation/schema/compare_elevation_map_filter.schema.json diff --git a/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json similarity index 100% rename from perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json rename to perception/autoware_compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json diff --git a/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json similarity index 100% rename from perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json rename to perception/autoware_compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json diff --git a/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json similarity index 100% rename from perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json rename to perception/autoware_compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json diff --git a/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json similarity index 100% rename from perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json rename to perception/autoware_compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/compare_elevation_map_filter/node.cpp similarity index 96% rename from perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp rename to perception/autoware_compare_map_segmentation/src/compare_elevation_map_filter/node.cpp index e5a19245bc5b1..73390abae7301 100644 --- a/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/compare_elevation_map_filter/node.cpp @@ -72,9 +72,7 @@ void CompareElevationMapFilterComponent::filter( { pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - std::string output_frame = map_frame_; - output_frame = elevation_map_.getFrameId(); elevation_map_.setTimestamp(input->header.stamp.nanosec); pcl::fromROSMsg(*input, *pcl_input); pcl_output->points.reserve(pcl_input->points.size()); @@ -92,7 +90,7 @@ void CompareElevationMapFilterComponent::filter( pcl::toROSMsg(*pcl_output, output); output.header.stamp = input->header.stamp; - output.header.frame_id = output_frame; + output.header.frame_id = elevation_map_.getFrameId(); } } // namespace autoware::compare_map_segmentation diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/compare_elevation_map_filter/node.hpp similarity index 93% rename from perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp rename to perception/autoware_compare_map_segmentation/src/compare_elevation_map_filter/node.hpp index ba2328d862951..ceddaa67ac2cd 100644 --- a/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/compare_elevation_map_filter/node.hpp @@ -15,7 +15,7 @@ #ifndef COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ #define COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include @@ -31,7 +31,7 @@ #include namespace autoware::compare_map_segmentation { -class CompareElevationMapFilterComponent : public pointcloud_preprocessor::Filter +class CompareElevationMapFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: void filter( diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp similarity index 100% rename from perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp rename to perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp similarity index 96% rename from perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp rename to perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp index 115a73cf0d263..d0cb5b3c62992 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp @@ -16,7 +16,7 @@ #define DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ #include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include // for pcl::isFinite #include @@ -100,7 +100,7 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader } }; -class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter +class DistanceBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp similarity index 100% rename from perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp rename to perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp similarity index 94% rename from perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp rename to perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp index 26b0204210209..0466d20403ca3 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp @@ -16,7 +16,7 @@ #define VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT #include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include @@ -56,7 +56,8 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override; }; -class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preprocessor::Filter +class VoxelBasedApproximateCompareMapFilterComponent +: public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp similarity index 98% rename from perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp rename to perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index 1861a710cdebd..a3eb866d77e31 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -26,7 +26,7 @@ namespace autoware::compare_map_segmentation { -using pointcloud_preprocessor::get_param; +using autoware::pointcloud_preprocessor::get_param; VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( const rclcpp::NodeOptions & options) diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp similarity index 91% rename from perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp rename to perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp index 6e03e5d7a2e09..ac81eae85b8a0 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp @@ -16,7 +16,7 @@ #define VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_ #include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include @@ -26,7 +26,7 @@ namespace autoware::compare_map_segmentation { -class VoxelBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter +class VoxelBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp similarity index 100% rename from perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp rename to perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp similarity index 97% rename from perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp rename to perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp index e0656359f3df2..15ed2a32ff213 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp @@ -16,7 +16,7 @@ #define VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT #include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp" -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include @@ -123,7 +123,7 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader } }; -class VoxelDistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter +class VoxelDistanceBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp similarity index 100% rename from perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp rename to perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp similarity index 98% rename from perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp rename to perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index b13f2e537ee55..07ed0aa7b92db 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -224,7 +224,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader } inline void updateDifferentialMapCells( const std::vector & map_cells_to_add, - std::vector map_cell_ids_to_remove) + const std::vector & map_cell_ids_to_remove) { for (const auto & map_cell_to_add : map_cells_to_add) { addMapCellAndFilter(map_cell_to_add); @@ -271,7 +271,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader (*mutex_ptr_).unlock(); } - inline void removeMapCell(const std::string map_cell_id_to_remove) + inline void removeMapCell(const std::string & map_cell_id_to_remove) { (*mutex_ptr_).lock(); current_voxel_grid_dict_.erase(map_cell_id_to_remove); 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/detected_object_feature_remover/CMakeLists.txt b/perception/autoware_detected_object_feature_remover/CMakeLists.txt similarity index 89% rename from perception/detected_object_feature_remover/CMakeLists.txt rename to perception/autoware_detected_object_feature_remover/CMakeLists.txt index e0ff35d75e64a..54763f8bff9ab 100644 --- a/perception/detected_object_feature_remover/CMakeLists.txt +++ b/perception/autoware_detected_object_feature_remover/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(detected_object_feature_remover) +project(autoware_detected_object_feature_remover) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/detected_object_feature_remover/README.md b/perception/autoware_detected_object_feature_remover/README.md similarity index 80% rename from perception/detected_object_feature_remover/README.md rename to perception/autoware_detected_object_feature_remover/README.md index 7ac85c2adfac5..2c4b75b71abb4 100644 --- a/perception/detected_object_feature_remover/README.md +++ b/perception/autoware_detected_object_feature_remover/README.md @@ -1,8 +1,8 @@ -# detected_object_feature_remover +# autoware_detected_object_feature_remover ## Purpose -The `detected_object_feature_remover` is a package to convert topic-type from `DetectedObjectWithFeatureArray` to `DetectedObjects`. +The `autoware_detected_object_feature_remover` is a package to convert topic-type from `DetectedObjectWithFeatureArray` to `DetectedObjects`. ## Inner-workings / Algorithms diff --git a/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml b/perception/autoware_detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml similarity index 62% rename from perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml rename to perception/autoware_detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml index d86d75ce297be..5e26dcfa9f696 100644 --- a/perception/detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml +++ b/perception/autoware_detected_object_feature_remover/launch/detected_object_feature_remover.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/detected_object_feature_remover/package.xml b/perception/autoware_detected_object_feature_remover/package.xml similarity index 85% rename from perception/detected_object_feature_remover/package.xml rename to perception/autoware_detected_object_feature_remover/package.xml index 9eb47e0e69557..a835e289cd455 100644 --- a/perception/detected_object_feature_remover/package.xml +++ b/perception/autoware_detected_object_feature_remover/package.xml @@ -1,9 +1,9 @@ - detected_object_feature_remover + autoware_detected_object_feature_remover 0.1.0 - The detected_object_feature_remover package + The autoware_detected_object_feature_remover package Tomoya Kimura Apache License 2.0 diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp similarity index 100% rename from perception/detected_object_feature_remover/src/detected_object_feature_remover_node.cpp rename to perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.cpp diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp b/perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.hpp similarity index 100% rename from perception/detected_object_feature_remover/src/detected_object_feature_remover_node.hpp rename to perception/autoware_detected_object_feature_remover/src/detected_object_feature_remover_node.hpp diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/autoware_detected_object_validation/CMakeLists.txt similarity index 98% rename from perception/detected_object_validation/CMakeLists.txt rename to perception/autoware_detected_object_validation/CMakeLists.txt index 6d93e7b426ad2..434c258918493 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/autoware_detected_object_validation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(detected_object_validation) +project(autoware_detected_object_validation) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/detected_object_validation/README.md b/perception/autoware_detected_object_validation/README.md similarity index 100% rename from perception/detected_object_validation/README.md rename to perception/autoware_detected_object_validation/README.md diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml similarity index 100% rename from perception/detected_object_validation/config/object_lanelet_filter.param.yaml rename to perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml diff --git a/perception/detected_object_validation/config/object_position_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_position_filter.param.yaml similarity index 100% rename from perception/detected_object_validation/config/object_position_filter.param.yaml rename to perception/autoware_detected_object_validation/config/object_position_filter.param.yaml diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml similarity index 100% rename from perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml rename to perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml diff --git a/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml similarity index 100% rename from perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml rename to perception/autoware_detected_object_validation/config/occupancy_grid_based_validator.param.yaml diff --git a/perception/detected_object_validation/image/obstacle_pointcloud_based_validator/debug_image.gif b/perception/autoware_detected_object_validation/image/obstacle_pointcloud_based_validator/debug_image.gif similarity index 100% rename from perception/detected_object_validation/image/obstacle_pointcloud_based_validator/debug_image.gif rename to perception/autoware_detected_object_validation/image/obstacle_pointcloud_based_validator/debug_image.gif diff --git a/perception/detected_object_validation/image/occupancy_grid_based_validator/debug_image.png b/perception/autoware_detected_object_validation/image/occupancy_grid_based_validator/debug_image.png similarity index 100% rename from perception/detected_object_validation/image/occupancy_grid_based_validator/debug_image.png rename to perception/autoware_detected_object_validation/image/occupancy_grid_based_validator/debug_image.png diff --git a/perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp b/perception/autoware_detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp similarity index 100% rename from perception/detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp rename to perception/autoware_detected_object_validation/include/autoware/detected_object_validation/utils/utils.hpp diff --git a/perception/detected_object_validation/launch/object_lanelet_filter.launch.xml b/perception/autoware_detected_object_validation/launch/object_lanelet_filter.launch.xml similarity index 62% rename from perception/detected_object_validation/launch/object_lanelet_filter.launch.xml rename to perception/autoware_detected_object_validation/launch/object_lanelet_filter.launch.xml index 2cccc4f3bb526..00cc961b17ee3 100644 --- a/perception/detected_object_validation/launch/object_lanelet_filter.launch.xml +++ b/perception/autoware_detected_object_validation/launch/object_lanelet_filter.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/detected_object_validation/launch/object_position_filter.launch.xml b/perception/autoware_detected_object_validation/launch/object_position_filter.launch.xml similarity index 53% rename from perception/detected_object_validation/launch/object_position_filter.launch.xml rename to perception/autoware_detected_object_validation/launch/object_position_filter.launch.xml index 49aae282aa3c3..03e7996bb41d1 100644 --- a/perception/detected_object_validation/launch/object_position_filter.launch.xml +++ b/perception/autoware_detected_object_validation/launch/object_position_filter.launch.xml @@ -2,9 +2,9 @@ - + - + diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/autoware_detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml similarity index 73% rename from perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml rename to perception/autoware_detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index a1d941e66db4b..d97b1a762d360 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/autoware_detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/autoware_detected_object_validation/launch/occupancy_grid_based_validator.launch.xml similarity index 69% rename from perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml rename to perception/autoware_detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 3acb1f2d1907a..dbc1a012975dd 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/autoware_detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -5,11 +5,11 @@ - - - + + + - + @@ -19,7 +19,7 @@ - + diff --git a/perception/detected_object_validation/lib/utils/utils.cpp b/perception/autoware_detected_object_validation/lib/utils/utils.cpp similarity index 100% rename from perception/detected_object_validation/lib/utils/utils.cpp rename to perception/autoware_detected_object_validation/lib/utils/utils.cpp diff --git a/perception/detected_object_validation/object-lanelet-filter.md b/perception/autoware_detected_object_validation/object-lanelet-filter.md similarity index 100% rename from perception/detected_object_validation/object-lanelet-filter.md rename to perception/autoware_detected_object_validation/object-lanelet-filter.md diff --git a/perception/detected_object_validation/object-position-filter.md b/perception/autoware_detected_object_validation/object-position-filter.md similarity index 100% rename from perception/detected_object_validation/object-position-filter.md rename to perception/autoware_detected_object_validation/object-position-filter.md diff --git a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md b/perception/autoware_detected_object_validation/obstacle-pointcloud-based-validator.md similarity index 100% rename from perception/detected_object_validation/obstacle-pointcloud-based-validator.md rename to perception/autoware_detected_object_validation/obstacle-pointcloud-based-validator.md diff --git a/perception/detected_object_validation/occupancy-grid-based-validator.md b/perception/autoware_detected_object_validation/occupancy-grid-based-validator.md similarity index 100% rename from perception/detected_object_validation/occupancy-grid-based-validator.md rename to perception/autoware_detected_object_validation/occupancy-grid-based-validator.md diff --git a/perception/detected_object_validation/package.xml b/perception/autoware_detected_object_validation/package.xml similarity index 96% rename from perception/detected_object_validation/package.xml rename to perception/autoware_detected_object_validation/package.xml index 09440c9f6358d..b46c182d2cb57 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/autoware_detected_object_validation/package.xml @@ -1,7 +1,7 @@ - detected_object_validation + autoware_detected_object_validation 1.0.0 The ROS 2 detected_object_validation package Yukihiro Saito diff --git a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp similarity index 63% rename from perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp rename to perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index 09440fedc1764..aed0ba5ab85ea 100644 --- a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -15,6 +15,7 @@ #include "lanelet_filter.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_lanelet2_extension/utility/message_conversion.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" @@ -22,7 +23,11 @@ #include #include #include +#include +#include +#include +#include #include namespace autoware::detected_object_validation @@ -76,9 +81,6 @@ void ObjectLaneletFilterNode::mapCallback( lanelet_frame_id_ = map_msg->header.frame_id; lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); - const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); - road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } void ObjectLaneletFilterNode::objectCallback( @@ -101,22 +103,27 @@ void ObjectLaneletFilterNode::objectCallback( return; } - // calculate convex hull - const auto convex_hull = getConvexHull(transformed_objects); - // get intersected lanelets - lanelet::ConstLanelets intersected_road_lanelets = - getIntersectedLanelets(convex_hull, road_lanelets_); - lanelet::ConstLanelets intersected_shoulder_lanelets = - getIntersectedLanelets(convex_hull, shoulder_lanelets_); - - // filtering process - for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { - const auto & transformed_object = transformed_objects.objects.at(index); - const auto & input_object = input_msg->objects.at(index); - filterObject( - transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, - output_object_msg); + if (!transformed_objects.objects.empty()) { + // calculate convex hull + const auto convex_hull = getConvexHull(transformed_objects); + + // get intersected lanelets + std::vector intersected_lanelets_with_bbox = getIntersectedLanelets(convex_hull); + + // create R-Tree with intersected_lanelets for fast search + bgi::rtree local_rtree; + for (const auto & bbox_and_lanelet : intersected_lanelets_with_bbox) { + local_rtree.insert(bbox_and_lanelet); + } + + // filtering process + for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { + const auto & transformed_object = transformed_objects.objects.at(index); + const auto & input_object = input_msg->objects.at(index); + filterObject(transformed_object, input_object, local_rtree, output_object_msg); + } } + object_pub_->publish(output_object_msg); published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); @@ -133,18 +140,20 @@ void ObjectLaneletFilterNode::objectCallback( bool ObjectLaneletFilterNode::filterObject( const autoware_perception_msgs::msg::DetectedObject & transformed_object, const autoware_perception_msgs::msg::DetectedObject & input_object, - const lanelet::ConstLanelets & intersected_road_lanelets, - const lanelet::ConstLanelets & intersected_shoulder_lanelets, + const bgi::rtree & local_rtree, autoware_perception_msgs::msg::DetectedObjects & output_object_msg) { const auto & label = transformed_object.classification.front().label; if (filter_target_.isTarget(label)) { + // no tree, then no intersection + if (local_rtree.empty()) { + return false; + } + bool filter_pass = true; // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { - const bool is_polygon_overlap = - isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || - isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); + const bool is_polygon_overlap = isObjectOverlapLanelets(transformed_object, local_rtree); filter_pass = filter_pass && is_polygon_overlap; } @@ -153,9 +162,7 @@ bool ObjectLaneletFilterNode::filterObject( transformed_object.kinematics.orientation_availability == autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { - const bool is_same_direction = - isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || - isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + const bool is_same_direction = isSameDirectionWithLanelets(transformed_object, local_rtree); filter_pass = filter_pass && is_same_direction; } @@ -207,33 +214,75 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull( candidate_points.emplace_back(p.x + pos.x, p.y + pos.y); } } - LinearRing2d convex_hull; - boost::geometry::convex_hull(candidate_points, convex_hull); + bg::convex_hull(candidate_points, convex_hull); return convex_hull; } -lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( - const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets) +LinearRing2d ObjectLaneletFilterNode::getConvexHullFromObjectFootprint( + const autoware_perception_msgs::msg::DetectedObject & object) { - lanelet::ConstLanelets intersected_lanelets; + MultiPoint2d candidate_points; + const auto & pos = object.kinematics.pose_with_covariance.pose.position; + const auto footprint = setFootprint(object); + + for (const auto & p : footprint.points) { + candidate_points.emplace_back(p.x + pos.x, p.y + pos.y); + } + + LinearRing2d convex_hull; + bg::convex_hull(candidate_points, convex_hull); - // WARNING: This implementation currently iterate all lanelets, which could degrade performance - // when handling large sized map. - for (const auto & road_lanelet : road_lanelets) { - if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { - intersected_lanelets.emplace_back(road_lanelet); + return convex_hull; +} + +// fetch the intersected candidate lanelets with bounding box and then +// check the intersections among the lanelets and the convex hull +std::vector ObjectLaneletFilterNode::getIntersectedLanelets( + const LinearRing2d & convex_hull) +{ + std::vector intersected_lanelets_with_bbox; + + // convert convex_hull to a 2D bounding box for searching in the LaneletMap + bg::model::box> bbox_of_convex_hull; + bg::envelope(convex_hull, bbox_of_convex_hull); + const lanelet::BoundingBox2d bbox2d( + lanelet::BasicPoint2d( + bg::get(bbox_of_convex_hull), + bg::get(bbox_of_convex_hull)), + lanelet::BasicPoint2d( + bg::get(bbox_of_convex_hull), + bg::get(bbox_of_convex_hull))); + + const lanelet::Lanelets candidate_lanelets = lanelet_map_ptr_->laneletLayer.search(bbox2d); + for (const auto & lanelet : candidate_lanelets) { + // only check the road lanelets and road shoulder lanelets + if ( + lanelet.hasAttribute(lanelet::AttributeName::Subtype) && + (lanelet.attribute(lanelet::AttributeName::Subtype).value() == + lanelet::AttributeValueString::Road || + lanelet.attribute(lanelet::AttributeName::Subtype).value() == "road_shoulder")) { + if (bg::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) { + // create bbox using boost for making the R-tree in later phase + lanelet::BoundingBox2d lanelet_bbox = lanelet::geometry::boundingBox2d(lanelet); + Point2d min_corner(lanelet_bbox.min().x(), lanelet_bbox.min().y()); + Point2d max_corner(lanelet_bbox.max().x(), lanelet_bbox.max().y()); + Box boost_bbox(min_corner, max_corner); + + intersected_lanelets_with_bbox.emplace_back(std::make_pair(boost_bbox, lanelet)); + } } } - return intersected_lanelets; + + return intersected_lanelets_with_bbox; } bool ObjectLaneletFilterNode::isObjectOverlapLanelets( const autoware_perception_msgs::msg::DetectedObject & object, - const lanelet::ConstLanelets & intersected_lanelets) + const bgi::rtree & local_rtree) { - // if has bounding box, use polygon overlap + // if object has bounding box, use polygon overlap if (utils::hasBoundingBox(object)) { Polygon2d polygon; const auto footprint = setFootprint(object); @@ -244,8 +293,17 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); - return isPolygonOverlapLanelets(polygon, intersected_lanelets); + + return isPolygonOverlapLanelets(polygon, local_rtree); } else { + const LinearRing2d object_convex_hull = getConvexHullFromObjectFootprint(object); + + // create bounding box to search in the rtree + std::vector candidates; + bg::model::box> bbox; + bg::envelope(object_convex_hull, bbox); + local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates)); + // if object do not have bounding box, check each footprint is inside polygon for (const auto & point : object.shape.footprint.points) { const geometry_msgs::msg::Point32 point_transformed = @@ -254,30 +312,39 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( geometry_msgs::msg::Pose point2d; point2d.position.x = point_transformed.x; point2d.position.y = point_transformed.y; - for (const auto & lanelet : intersected_lanelets) { - if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) { + + for (const auto & candidate : candidates) { + if (lanelet::utils::isInLanelet(point2d, candidate.second, 0.0)) { return true; } } } + return false; } } bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( - const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) + const Polygon2d & polygon, const bgi::rtree & local_rtree) { - for (const auto & lanelet : intersected_lanelets) { - if (!boost::geometry::disjoint(polygon, lanelet.polygon2d().basicPolygon())) { + // create a bounding box from polygon for searching the local R-tree + std::vector candidates; + bg::model::box> bbox_of_convex_hull; + bg::envelope(polygon, bbox_of_convex_hull); + local_rtree.query(bgi::intersects(bbox_of_convex_hull), std::back_inserter(candidates)); + + for (const auto & box_and_lanelet : candidates) { + if (!bg::disjoint(polygon, box_and_lanelet.second.polygon2d().basicPolygon())) { return true; } } + return false; } bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( - const lanelet::ConstLanelets & lanelets, - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_perception_msgs::msg::DetectedObject & object, + const bgi::rtree & local_rtree) { const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double object_velocity_norm = std::hypot( @@ -291,14 +358,30 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) { return true; } - for (const auto & lanelet : lanelets) { - const bool is_in_lanelet = - lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0); + + // we can not query by points, so create a small bounding box + // eps is not determined by any specific criteria; just a guess + constexpr double eps = 1.0e-6; + std::vector candidates; + const Point2d min_corner( + object.kinematics.pose_with_covariance.pose.position.x - eps, + object.kinematics.pose_with_covariance.pose.position.y - eps); + const Point2d max_corner( + object.kinematics.pose_with_covariance.pose.position.x + eps, + object.kinematics.pose_with_covariance.pose.position.y + eps); + const Box bbox(min_corner, max_corner); + + local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates)); + + for (const auto & box_and_lanelet : candidates) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.kinematics.pose_with_covariance.pose, box_and_lanelet.second, 0.0); if (!is_in_lanelet) { continue; } + const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet, object.kinematics.pose_with_covariance.pose.position); + box_and_lanelet.second, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_velocity_yaw - lane_yaw; const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); diff --git a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp similarity index 79% rename from perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp rename to perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 25d78a160c246..67713e86de71e 100644 --- a/perception/detected_object_validation/src/lanelet_filter/lanelet_filter.hpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -26,12 +26,17 @@ #include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" +#include + #include +#include #include #include #include #include +#include +#include namespace autoware::detected_object_validation { @@ -42,6 +47,13 @@ using autoware::universe_utils::MultiPoint2d; using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; +namespace bg = boost::geometry; +namespace bgi = boost::geometry::index; +using Point2d = bg::model::point; +using Box = boost::geometry::model::box; +using BoxAndLanelet = std::pair; +using RtreeAlgo = bgi::rstar<16>; + class ObjectLaneletFilterNode : public rclcpp::Node { public: @@ -57,8 +69,6 @@ class ObjectLaneletFilterNode : public rclcpp::Node std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::ConstLanelets road_lanelets_; - lanelet::ConstLanelets shoulder_lanelets_; std::string lanelet_frame_id_; tf2_ros::Buffer tf_buffer_; @@ -76,19 +86,20 @@ class ObjectLaneletFilterNode : public rclcpp::Node bool filterObject( const autoware_perception_msgs::msg::DetectedObject & transformed_object, const autoware_perception_msgs::msg::DetectedObject & input_object, - const lanelet::ConstLanelets & intersected_road_lanelets, - const lanelet::ConstLanelets & intersected_shoulder_lanelets, + const bg::index::rtree & local_rtree, autoware_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_perception_msgs::msg::DetectedObjects &); - lanelet::ConstLanelets getIntersectedLanelets( - const LinearRing2d &, const lanelet::ConstLanelets &); + LinearRing2d getConvexHullFromObjectFootprint( + const autoware_perception_msgs::msg::DetectedObject & object); + std::vector getIntersectedLanelets(const LinearRing2d &); bool isObjectOverlapLanelets( const autoware_perception_msgs::msg::DetectedObject & object, - const lanelet::ConstLanelets & intersected_lanelets); - bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + const bg::index::rtree & local_rtree); + bool isPolygonOverlapLanelets( + const Polygon2d & polygon, const bgi::rtree & local_rtree); bool isSameDirectionWithLanelets( - const lanelet::ConstLanelets & lanelets, - const autoware_perception_msgs::msg::DetectedObject & object); + const autoware_perception_msgs::msg::DetectedObject & object, + const bgi::rtree & local_rtree); geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); std::unique_ptr published_time_publisher_; diff --git a/perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp similarity index 100% rename from perception/detected_object_validation/src/obstacle_pointcloud/debugger.hpp rename to perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp diff --git a/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp similarity index 100% rename from perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp rename to perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp diff --git a/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp similarity index 99% rename from perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp rename to perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index 801a693905f93..75f513ba803c2 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -65,7 +65,7 @@ class Validator public: explicit Validator(const PointsNumThresholdParam & points_num_threshold_param); - inline pcl::PointCloud::Ptr getDebugPointCloudWithinObject() + inline pcl::PointCloud::Ptr getDebugPointCloudWithinObject() const { return cropped_pointcloud_; } diff --git a/perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp similarity index 95% rename from perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.cpp rename to perception/autoware_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/autoware_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/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp b/perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp similarity index 100% rename from perception/detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp rename to perception/autoware_detected_object_validation/src/occupancy_grid_map/occupancy_grid_map_validator.hpp diff --git a/perception/detected_object_validation/src/position_filter/position_filter.cpp b/perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp similarity index 100% rename from perception/detected_object_validation/src/position_filter/position_filter.cpp rename to perception/autoware_detected_object_validation/src/position_filter/position_filter.cpp diff --git a/perception/detected_object_validation/src/position_filter/position_filter.hpp b/perception/autoware_detected_object_validation/src/position_filter/position_filter.hpp similarity index 100% rename from perception/detected_object_validation/src/position_filter/position_filter.hpp rename to perception/autoware_detected_object_validation/src/position_filter/position_filter.hpp diff --git a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp b/perception/autoware_detected_object_validation/test/object_position_filter/test_object_position_filter.cpp similarity index 98% rename from perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp rename to perception/autoware_detected_object_validation/test/object_position_filter/test_object_position_filter.cpp index 5bd174d5a9e17..7a6169254348c 100644 --- a/perception/detected_object_validation/test/object_position_filter/test_object_position_filter.cpp +++ b/perception/autoware_detected_object_validation/test/object_position_filter/test_object_position_filter.cpp @@ -36,7 +36,7 @@ std::shared_ptr generateNode() { auto node_options = rclcpp::NodeOptions{}; const auto detected_object_validation_dir = - ament_index_cpp::get_package_share_directory("detected_object_validation"); + ament_index_cpp::get_package_share_directory("autoware_detected_object_validation"); node_options.arguments( {"--ros-args", "--params-file", detected_object_validation_dir + "/config/object_position_filter.param.yaml"}); diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/autoware_detected_object_validation/test/test_utils.cpp similarity index 100% rename from perception/detected_object_validation/test/test_utils.cpp rename to perception/autoware_detected_object_validation/test/test_utils.cpp diff --git a/perception/autoware_detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml index 837dfae1e2ef9..6f23baeefbd3e 100644 --- a/perception/autoware_detection_by_tracker/package.xml +++ b/perception/autoware_detection_by_tracker/package.xml @@ -13,13 +13,13 @@ autoware_cmake eigen3_cmake_module + autoware_euclidean_cluster + autoware_shape_estimation autoware_universe_utils eigen - euclidean_cluster object_recognition_utils rclcpp rclcpp_components - shape_estimation tf2 tf2_geometry_msgs tf2_ros diff --git a/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp index a099c8b1ba55d..9c7d801332bb8 100644 --- a/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp +++ b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp @@ -23,12 +23,12 @@ namespace autoware::detection_by_tracker { void TrackerHandler::onTrackedObjects( - const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg) { constexpr size_t max_buffer_size = 10; // Add tracked objects to buffer - objects_buffer_.push_front(*msg); + objects_buffer_.push_front(*input_objects_msg); // Remove old data while (max_buffer_size < objects_buffer_.size()) { diff --git a/perception/elevation_map_loader/CMakeLists.txt b/perception/autoware_elevation_map_loader/CMakeLists.txt similarity index 56% rename from perception/elevation_map_loader/CMakeLists.txt rename to perception/autoware_elevation_map_loader/CMakeLists.txt index 6bfe60ef21062..f84ed01040f7f 100644 --- a/perception/elevation_map_loader/CMakeLists.txt +++ b/perception/autoware_elevation_map_loader/CMakeLists.txt @@ -1,27 +1,27 @@ cmake_minimum_required(VERSION 3.14) -project(elevation_map_loader) +project(autoware_elevation_map_loader) find_package(autoware_cmake REQUIRED) autoware_package() find_package(PCL REQUIRED COMPONENTS io) -ament_auto_add_library(elevation_map_loader_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/elevation_map_loader_node.cpp ) -target_link_libraries(elevation_map_loader_node ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) # TODO(wep21): workaround for iron. # remove this block and update package.xml after iron. find_package(rosbag2_storage_sqlite3) -target_include_directories(elevation_map_loader_node +target_include_directories(${PROJECT_NAME} PRIVATE ${rosbag2_storage_sqlite3_INCLUDE_DIRS} ) -rclcpp_components_register_node(elevation_map_loader_node - PLUGIN "ElevationMapLoaderNode" - EXECUTABLE elevation_map_loader +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::elevation_map_loader::ElevationMapLoaderNode" + EXECUTABLE elevation_map_loader_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/elevation_map_loader/README.md b/perception/autoware_elevation_map_loader/README.md similarity index 99% rename from perception/elevation_map_loader/README.md rename to perception/autoware_elevation_map_loader/README.md index 6920ed927a18a..03d5fac308dab 100644 --- a/perception/elevation_map_loader/README.md +++ b/perception/autoware_elevation_map_loader/README.md @@ -1,8 +1,8 @@ -# elevation_map_loader +# autoware_elevation_map_loader ## Purpose -This package provides elevation map for compare_map_segmentation. +This package provides elevation map for autoware_compare_map_segmentation. ## Inner-workings / Algorithms diff --git a/perception/elevation_map_loader/config/elevation_map_parameters.yaml b/perception/autoware_elevation_map_loader/config/elevation_map_parameters.yaml similarity index 100% rename from perception/elevation_map_loader/config/elevation_map_parameters.yaml rename to perception/autoware_elevation_map_loader/config/elevation_map_parameters.yaml diff --git a/perception/elevation_map_loader/data/elevation_maps/.empty b/perception/autoware_elevation_map_loader/data/elevation_maps/.empty similarity index 100% rename from perception/elevation_map_loader/data/elevation_maps/.empty rename to perception/autoware_elevation_map_loader/data/elevation_maps/.empty diff --git a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml b/perception/autoware_elevation_map_loader/launch/elevation_map_loader.launch.xml similarity index 77% rename from perception/elevation_map_loader/launch/elevation_map_loader.launch.xml rename to perception/autoware_elevation_map_loader/launch/elevation_map_loader.launch.xml index b222d11c634a7..42cf0815b723f 100644 --- a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml +++ b/perception/autoware_elevation_map_loader/launch/elevation_map_loader.launch.xml @@ -1,13 +1,13 @@ - - + + - + diff --git a/perception/elevation_map_loader/media/elevation_map.png b/perception/autoware_elevation_map_loader/media/elevation_map.png similarity index 100% rename from perception/elevation_map_loader/media/elevation_map.png rename to perception/autoware_elevation_map_loader/media/elevation_map.png diff --git a/perception/elevation_map_loader/package.xml b/perception/autoware_elevation_map_loader/package.xml similarity index 89% rename from perception/elevation_map_loader/package.xml rename to perception/autoware_elevation_map_loader/package.xml index 08990b971b0a3..ca8e25d5f333e 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/autoware_elevation_map_loader/package.xml @@ -1,9 +1,9 @@ - elevation_map_loader + autoware_elevation_map_loader 0.1.0 - The map_loader package + The autoware_elevation_map_loader package Kosuke Takeuchi Taichi Higashide Shintaro Tomie @@ -24,8 +24,6 @@ rclcpp rclcpp_components rosbag2_storage_default_plugins - tf2_geometry_msgs - tf2_ros tier4_external_api_msgs ament_lint_auto diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp similarity index 98% rename from perception/elevation_map_loader/src/elevation_map_loader_node.cpp rename to perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp index 80f7d85fd53c8..8eed81155cf96 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.cpp @@ -12,9 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "elevation_map_loader/elevation_map_loader_node.hpp" +#define EIGEN_MPL2_ONLY + +#include "elevation_map_loader_node.hpp" + +#include "autoware_grid_map_utils/polygon_iterator.hpp" -#include +#include +#include #include #include #include @@ -45,16 +50,15 @@ #include #include -#define EIGEN_MPL2_ONLY -#include -#include - #if __has_include() #include #else #include #endif +namespace autoware::elevation_map_loader +{ + ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & options) : Node("elevation_map_loader", options) { @@ -445,6 +449,7 @@ void ElevationMapLoaderNode::saveElevationMap() RCLCPP_INFO_STREAM( this->get_logger(), "Saving elevation map successful: " << std::boolalpha << saving_successful); } +} // namespace autoware::elevation_map_loader #include -RCLCPP_COMPONENTS_REGISTER_NODE(ElevationMapLoaderNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::elevation_map_loader::ElevationMapLoaderNode) diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.hpp similarity index 94% rename from perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp rename to perception/autoware_elevation_map_loader/src/elevation_map_loader_node.hpp index fda0fcddc1bc7..2a0cdc0dd2b15 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/autoware_elevation_map_loader/src/elevation_map_loader_node.hpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ -#define ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ +#ifndef ELEVATION_MAP_LOADER_NODE_HPP_ +#define ELEVATION_MAP_LOADER_NODE_HPP_ + +#include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include #include #include #include @@ -37,6 +38,9 @@ #include #include +namespace autoware::elevation_map_loader +{ + class DataManager { public: @@ -117,4 +121,5 @@ class ElevationMapLoaderNode : public rclcpp::Node LaneFilter lane_filter_; }; -#endif // ELEVATION_MAP_LOADER__ELEVATION_MAP_LOADER_NODE_HPP_ +} // namespace autoware::elevation_map_loader +#endif // ELEVATION_MAP_LOADER_NODE_HPP_ diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/autoware_euclidean_cluster/CMakeLists.txt similarity index 97% rename from perception/euclidean_cluster/CMakeLists.txt rename to perception/autoware_euclidean_cluster/CMakeLists.txt index d27390ae7707a..55f664489a4d9 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/autoware_euclidean_cluster/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(euclidean_cluster) +project(autoware_euclidean_cluster) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/euclidean_cluster/README.md b/perception/autoware_euclidean_cluster/README.md similarity index 97% rename from perception/euclidean_cluster/README.md rename to perception/autoware_euclidean_cluster/README.md index b57204c84cc3b..349d4211ee8e9 100644 --- a/perception/euclidean_cluster/README.md +++ b/perception/autoware_euclidean_cluster/README.md @@ -1,8 +1,8 @@ -# euclidean_cluster +# autoware_euclidean_cluster ## Purpose -euclidean_cluster is a package for clustering points into smaller parts to classify objects. +autoware_euclidean_cluster is a package for clustering points into smaller parts to classify objects. This package has two clustering methods: `euclidean_cluster` and `voxel_grid_based_euclidean_cluster`. diff --git a/perception/euclidean_cluster/config/euclidean_cluster.param.yaml b/perception/autoware_euclidean_cluster/config/euclidean_cluster.param.yaml similarity index 100% rename from perception/euclidean_cluster/config/euclidean_cluster.param.yaml rename to perception/autoware_euclidean_cluster/config/euclidean_cluster.param.yaml diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/autoware_euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml similarity index 100% rename from perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml rename to perception/autoware_euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster.hpp diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/euclidean_cluster_interface.hpp diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/utils.hpp diff --git a/perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp similarity index 100% rename from perception/euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp rename to perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.py similarity index 96% rename from perception/euclidean_cluster/launch/euclidean_cluster.launch.py rename to perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.py index 053438a4352b0..fbbd70b966809 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.py @@ -33,12 +33,12 @@ def load_composable_node_param(param_path): return yaml.safe_load(f)["/**"]["ros__parameters"] ns = "" - pkg = "euclidean_cluster" + pkg = "autoware_euclidean_cluster" low_height_cropbox_filter_component = ComposableNode( - package="pointcloud_preprocessor", + package="autoware_pointcloud_preprocessor", namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.xml similarity index 80% rename from perception/euclidean_cluster/launch/euclidean_cluster.launch.xml rename to perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.xml index f4deeccf7b76c..22d62c4df375a 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/autoware_euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -4,11 +4,11 @@ - + - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py similarity index 96% rename from perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py rename to perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 0b502b1c43d67..6a678cc4c7f59 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -32,12 +32,12 @@ def load_composable_node_param(param_path): return yaml.safe_load(f)["/**"]["ros__parameters"] ns = "" - pkg = "euclidean_cluster" + pkg = "autoware_euclidean_cluster" low_height_cropbox_filter_component = ComposableNode( - package="pointcloud_preprocessor", + package="autoware_pointcloud_preprocessor", namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml similarity index 82% rename from perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml rename to perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 3a7c685d8f449..c9ece95e9a606 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/autoware_euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -4,11 +4,11 @@ - + - + diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp similarity index 100% rename from perception/euclidean_cluster/lib/euclidean_cluster.cpp rename to perception/autoware_euclidean_cluster/lib/euclidean_cluster.cpp diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/autoware_euclidean_cluster/lib/utils.cpp similarity index 100% rename from perception/euclidean_cluster/lib/utils.cpp rename to perception/autoware_euclidean_cluster/lib/utils.cpp diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp similarity index 100% rename from perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp rename to perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp diff --git a/perception/euclidean_cluster/package.xml b/perception/autoware_euclidean_cluster/package.xml similarity index 90% rename from perception/euclidean_cluster/package.xml rename to perception/autoware_euclidean_cluster/package.xml index 45b5d05031c72..076406bd3980a 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/autoware_euclidean_cluster/package.xml @@ -1,9 +1,9 @@ - euclidean_cluster + autoware_euclidean_cluster 0.1.0 - The euclidean_cluster package + The autoware_euclidean_cluster package Yukihiro Saito Dai Nguyen Apache License 2.0 diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp similarity index 100% rename from perception/euclidean_cluster/src/euclidean_cluster_node.cpp rename to perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp similarity index 100% rename from perception/euclidean_cluster/src/euclidean_cluster_node.hpp rename to perception/autoware_euclidean_cluster/src/euclidean_cluster_node.hpp diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp similarity index 100% rename from perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp rename to perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp similarity index 100% rename from perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp rename to perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp diff --git a/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp similarity index 100% rename from perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp rename to perception/autoware_euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/autoware_ground_segmentation/CMakeLists.txt similarity index 97% rename from perception/ground_segmentation/CMakeLists.txt rename to perception/autoware_ground_segmentation/CMakeLists.txt index f8e4f50a553a2..9cf256b9492bb 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/autoware_ground_segmentation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(ground_segmentation) +project(autoware_ground_segmentation) find_package(autoware_cmake REQUIRED) autoware_package() @@ -104,7 +104,7 @@ if(BUILD_TESTING) test/test_ransac_ground_filter.cpp ) target_link_libraries(test_ransac_ground_filter - ground_segmentation + autoware_ground_segmentation ${YAML_CPP_LIBRARIES}) endif() diff --git a/perception/ground_segmentation/README.md b/perception/autoware_ground_segmentation/README.md similarity index 89% rename from perception/ground_segmentation/README.md rename to perception/autoware_ground_segmentation/README.md index ec52e33e6db0a..b981b02e0a844 100644 --- a/perception/ground_segmentation/README.md +++ b/perception/autoware_ground_segmentation/README.md @@ -1,8 +1,8 @@ -# ground_segmentation +# autoware_ground_segmentation ## Purpose -The `ground_segmentation` is a node that remove the ground points from the input pointcloud. +The `autoware_ground_segmentation` is a node that remove the ground points from the input pointcloud. ## Inner-workings / Algorithms @@ -44,7 +44,7 @@ Detail description of each ground segmentation algorithm is in the following lin ## Assumptions / Known limits -`pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). +`autoware::pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). ## References/External links diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/autoware_ground_segmentation/config/ground_segmentation.param.yaml similarity index 100% rename from perception/ground_segmentation/config/ground_segmentation.param.yaml rename to perception/autoware_ground_segmentation/config/ground_segmentation.param.yaml diff --git a/perception/ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/autoware_ground_segmentation/config/ransac_ground_filter.param.yaml similarity index 100% rename from perception/ground_segmentation/config/ransac_ground_filter.param.yaml rename to perception/autoware_ground_segmentation/config/ransac_ground_filter.param.yaml diff --git a/perception/ground_segmentation/config/ray_ground_filter.param.yaml b/perception/autoware_ground_segmentation/config/ray_ground_filter.param.yaml similarity index 100% rename from perception/ground_segmentation/config/ray_ground_filter.param.yaml rename to perception/autoware_ground_segmentation/config/ray_ground_filter.param.yaml diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml similarity index 100% rename from perception/ground_segmentation/config/scan_ground_filter.param.yaml rename to perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml diff --git a/perception/ground_segmentation/docs/image/ground_filter-ray-xy.drawio.svg b/perception/autoware_ground_segmentation/docs/image/ground_filter-ray-xy.drawio.svg similarity index 100% rename from perception/ground_segmentation/docs/image/ground_filter-ray-xy.drawio.svg rename to perception/autoware_ground_segmentation/docs/image/ground_filter-ray-xy.drawio.svg diff --git a/perception/ground_segmentation/docs/image/ground_filter-ray-xz.drawio.svg b/perception/autoware_ground_segmentation/docs/image/ground_filter-ray-xz.drawio.svg similarity index 100% rename from perception/ground_segmentation/docs/image/ground_filter-ray-xz.drawio.svg rename to perception/autoware_ground_segmentation/docs/image/ground_filter-ray-xz.drawio.svg diff --git a/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg b/perception/autoware_ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg similarity index 100% rename from perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg rename to perception/autoware_ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg diff --git a/perception/ground_segmentation/docs/ransac-ground-filter.md b/perception/autoware_ground_segmentation/docs/ransac-ground-filter.md similarity index 90% rename from perception/ground_segmentation/docs/ransac-ground-filter.md rename to perception/autoware_ground_segmentation/docs/ransac-ground-filter.md index 8bec24ea34d0c..a11a35521d173 100644 --- a/perception/ground_segmentation/docs/ransac-ground-filter.md +++ b/perception/autoware_ground_segmentation/docs/ransac-ground-filter.md @@ -10,13 +10,13 @@ Apply the input points to the plane, and set the points at a certain distance fr ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Node Parameters -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). #### Core Parameters diff --git a/perception/ground_segmentation/docs/ray-ground-filter.md b/perception/autoware_ground_segmentation/docs/ray-ground-filter.md similarity index 95% rename from perception/ground_segmentation/docs/ray-ground-filter.md rename to perception/autoware_ground_segmentation/docs/ray-ground-filter.md index 4774939a6a367..9b80b0588f851 100644 --- a/perception/ground_segmentation/docs/ray-ground-filter.md +++ b/perception/autoware_ground_segmentation/docs/ray-ground-filter.md @@ -12,13 +12,13 @@ The points is separated radially (Ray), and the ground is classified for each Ra ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Node Parameters -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Core Parameters diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/autoware_ground_segmentation/docs/scan-ground-filter.md similarity index 97% rename from perception/ground_segmentation/docs/scan-ground-filter.md rename to perception/autoware_ground_segmentation/docs/scan-ground-filter.md index d4eb9c6f3addf..bdd46cffb7df4 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/autoware_ground_segmentation/docs/scan-ground-filter.md @@ -19,13 +19,13 @@ This algorithm works by following steps, ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Node Parameters -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). #### Core Parameters diff --git a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml b/perception/autoware_ground_segmentation/launch/ransac_ground_filter.launch.xml similarity index 68% rename from perception/ground_segmentation/launch/ransac_ground_filter.launch.xml rename to perception/autoware_ground_segmentation/launch/ransac_ground_filter.launch.xml index f8280b774c6b0..fff8634b0df42 100644 --- a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml +++ b/perception/autoware_ground_segmentation/launch/ransac_ground_filter.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml b/perception/autoware_ground_segmentation/launch/ray_ground_filter.launch.xml similarity index 69% rename from perception/ground_segmentation/launch/ray_ground_filter.launch.xml rename to perception/autoware_ground_segmentation/launch/ray_ground_filter.launch.xml index 86375c520a426..94e09fb61142c 100644 --- a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml +++ b/perception/autoware_ground_segmentation/launch/ray_ground_filter.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py similarity index 98% rename from perception/ground_segmentation/launch/scan_ground_filter.launch.py rename to perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py index 990fcace3449a..6c85fb9302fdc 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py @@ -35,7 +35,7 @@ def launch_setup(context, *args, **kwargs): nodes = [ ComposableNode( - package="ground_segmentation", + package="autoware_ground_segmentation", plugin="autoware::ground_segmentation::ScanGroundFilterComponent", name="scan_ground_filter", remappings=[ diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.xml similarity index 76% rename from perception/ground_segmentation/launch/scan_ground_filter.launch.xml rename to perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.xml index 1146138211211..6a2073675e839 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml +++ b/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.xml @@ -1,12 +1,12 @@ - + - + diff --git a/perception/ground_segmentation/package.xml b/perception/autoware_ground_segmentation/package.xml similarity index 89% rename from perception/ground_segmentation/package.xml rename to perception/autoware_ground_segmentation/package.xml index 1b807b393b739..1a2f1d481a6c8 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/autoware_ground_segmentation/package.xml @@ -1,9 +1,9 @@ - ground_segmentation + autoware_ground_segmentation 0.1.0 - The ROS 2 ground_segmentation package + The ROS 2 autoware_ground_segmentation package amc-nu Yukihiro Saito Shunsuke Miura @@ -19,11 +19,11 @@ ament_cmake_auto autoware_cmake + autoware_pointcloud_preprocessor autoware_vehicle_info_utils libopencv-dev pcl_conversions pcl_ros - pointcloud_preprocessor rclcpp rclcpp_components sensor_msgs diff --git a/perception/ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp similarity index 99% rename from perception/ground_segmentation/src/ransac_ground_filter/node.cpp rename to perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp index 25e29c5b9f21a..dd5ce96a858cd 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp @@ -80,7 +80,7 @@ PlaneBasis getPlaneBasis(const Eigen::Vector3d & plane_normal) return basis; } -using pointcloud_preprocessor::get_param; +using autoware::pointcloud_preprocessor::get_param; RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RANSACGroundFilter", options) diff --git a/perception/ground_segmentation/src/ransac_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp similarity index 96% rename from perception/ground_segmentation/src/ransac_ground_filter/node.hpp rename to perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp index ebdd28f26ebe8..e173fe131602f 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp @@ -15,7 +15,7 @@ #ifndef RANSAC_GROUND_FILTER__NODE_HPP_ #define RANSAC_GROUND_FILTER__NODE_HPP_ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include @@ -54,7 +54,7 @@ struct RGB double b = 0.0; }; -class RANSACGroundFilterComponent : public pointcloud_preprocessor::Filter +class RANSACGroundFilterComponent : public autoware::pointcloud_preprocessor::Filter { using PointType = pcl::PointXYZ; diff --git a/perception/ground_segmentation/src/ray_ground_filter/gencolors.hpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/gencolors.hpp similarity index 100% rename from perception/ground_segmentation/src/ray_ground_filter/gencolors.hpp rename to perception/autoware_ground_segmentation/src/ray_ground_filter/gencolors.hpp diff --git a/perception/ground_segmentation/src/ray_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp similarity index 99% rename from perception/ground_segmentation/src/ray_ground_filter/node.cpp rename to perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp index f07db15615739..fd8fcd4dc29e7 100644 --- a/perception/ground_segmentation/src/ray_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp @@ -36,7 +36,7 @@ namespace autoware::ground_segmentation { -using pointcloud_preprocessor::get_param; +using autoware::pointcloud_preprocessor::get_param; RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RayGroundFilter", options) diff --git a/perception/ground_segmentation/src/ray_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp similarity index 97% rename from perception/ground_segmentation/src/ray_ground_filter/node.hpp rename to perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp index d3be2e60c4262..54de09b37ad96 100644 --- a/perception/ground_segmentation/src/ray_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp @@ -58,8 +58,8 @@ #include #endif +#include "autoware/pointcloud_preprocessor/filter.hpp" #include "gencolors.hpp" -#include "pointcloud_preprocessor/filter.hpp" #include #include @@ -81,7 +81,7 @@ using Polygon = bg::model::polygon; namespace autoware::ground_segmentation { -class RayGroundFilterComponent : public pointcloud_preprocessor::Filter +class RayGroundFilterComponent : public autoware::pointcloud_preprocessor::Filter { typedef pcl::PointXYZ PointType_; @@ -186,8 +186,7 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter */ void ExtractPointsIndices( 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); + PointCloud2::SharedPtr ground_cloud_msg_ptr, PointCloud2::SharedPtr no_ground_cloud_msg_ptr); boost::optional calcPointVehicleIntersection(const Point & point); diff --git a/perception/ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp similarity index 99% rename from perception/ground_segmentation/src/scan_ground_filter/node.cpp rename to perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 01116c5bdce5f..2745606697a20 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -25,15 +25,15 @@ namespace autoware::ground_segmentation { +using autoware::pointcloud_preprocessor::get_param; using autoware::universe_utils::calcDistance3d; using autoware::universe_utils::deg2rad; using autoware::universe_utils::normalizeDegree; using autoware::universe_utils::normalizeRadian; using autoware::vehicle_info_utils::VehicleInfoUtils; -using pointcloud_preprocessor::get_param; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) -: pointcloud_preprocessor::Filter("ScanGroundFilter", options) +: autoware::pointcloud_preprocessor::Filter("ScanGroundFilter", options) { // set initial parameters { @@ -582,7 +582,7 @@ void ScanGroundFilterComponent::extractObjectPoints( void ScanGroundFilterComponent::faster_filter( const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, PointCloud2 & output, - [[maybe_unused]] const pointcloud_preprocessor::TransformInfo & transform_info) + [[maybe_unused]] const autoware::pointcloud_preprocessor::TransformInfo & transform_info) { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); diff --git a/perception/ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp similarity index 93% rename from perception/ground_segmentation/src/scan_ground_filter/node.hpp rename to perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index 67a844eee75c5..ad5d0ea4b6bc6 100644 --- a/perception/ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -15,9 +15,9 @@ #ifndef SCAN_GROUND_FILTER__NODE_HPP_ #define SCAN_GROUND_FILTER__NODE_HPP_ +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" #include "autoware_vehicle_info_utils/vehicle_info.hpp" -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" #include @@ -44,7 +44,7 @@ namespace autoware::ground_segmentation { using autoware::vehicle_info_utils::VehicleInfo; -class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter +class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filter { private: // classified point label @@ -152,7 +152,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter // conform to new API virtual void faster_filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, - const pointcloud_preprocessor::TransformInfo & transform_info); + const autoware::pointcloud_preprocessor::TransformInfo & transform_info); tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; @@ -211,15 +211,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter /*! * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector * @param[in] in_cloud Input Point Cloud to be organized in radial segments - * @param[out] out_radial_ordered_points_manager Vector of Points Clouds, + * @param[out] out_radial_ordered_points Vector of Points Clouds, * each element will contain the points ordered */ void convertPointcloud( const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points_manager); + std::vector & out_radial_ordered_points); void convertPointcloudGridScan( const PointCloud2ConstPtr & in_cloud, - std::vector & out_radial_ordered_points_manager); + std::vector & out_radial_ordered_points); /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point @@ -249,12 +249,10 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter 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, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( - const PointCloud2ConstPtr & in_cloud_ptr, - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); /*! * Re-classifies point of ground cluster based on their height diff --git a/perception/ground_segmentation/test/data/test.pcd b/perception/autoware_ground_segmentation/test/data/test.pcd similarity index 100% rename from perception/ground_segmentation/test/data/test.pcd rename to perception/autoware_ground_segmentation/test/data/test.pcd diff --git a/perception/ground_segmentation/test/test_ransac_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp similarity index 98% rename from perception/ground_segmentation/test/test_ransac_ground_filter.cpp rename to perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp index fb66ba9e05b6d..e18c166adb8b3 100644 --- a/perception/ground_segmentation/test/test_ransac_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp @@ -134,7 +134,8 @@ void convertPCL2PointCloud2( TEST_F(RansacGroundFilterTestSuite, TestCase1) { - const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_ground_segmentation"); const auto config_path = share_dir + "/config/ransac_ground_filter.param.yaml"; YAML::Node config = YAML::LoadFile(config_path); auto params = config["/**"]["ros__parameters"]; diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp similarity index 98% rename from perception/ground_segmentation/test/test_ray_ground_filter.cpp rename to perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp index 4233b8bb18ebe..81efeff8111cb 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp @@ -102,7 +102,8 @@ void convertPCL2PointCloud2( TEST_F(RayGroundFilterComponentTestSuite, TestCase1) { - const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_ground_segmentation"); const auto config_path = share_dir + "/config/ray_ground_filter.param.yaml"; // std::cout << "config_path:" << config_path << std::endl; YAML::Node config = YAML::LoadFile(config_path); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp similarity index 97% rename from perception/ground_segmentation/test/test_scan_ground_filter.cpp rename to perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp index f5faecd59d6d3..c6603dfd9a3e2 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp @@ -131,7 +131,8 @@ class ScanGroundFilterTest : public ::testing::Test // read pcd to pointcloud sensor_msgs::msg::PointCloud2::SharedPtr origin_input_msg_ptr = std::make_shared(); - const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_ground_segmentation"); const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; pcl::io::loadPCDFile(pcd_path, cloud); @@ -171,13 +172,14 @@ class ScanGroundFilterTest : public ::testing::Test // wrapper function to test private function filter void filter(sensor_msgs::msg::PointCloud2 & out_cloud) { - pointcloud_preprocessor::TransformInfo transform_info; + autoware::pointcloud_preprocessor::TransformInfo transform_info; scan_ground_filter_->faster_filter(input_msg_ptr_, nullptr, out_cloud, transform_info); } void parse_yaml() { - const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_ground_segmentation"); const auto config_path = share_dir + "/config/scan_ground_filter.param.yaml"; // std::cout << "config_path:" << config_path << std::endl; YAML::Node config = YAML::LoadFile(config_path); diff --git a/perception/image_projection_based_fusion/.gitignore b/perception/autoware_image_projection_based_fusion/.gitignore similarity index 100% rename from perception/image_projection_based_fusion/.gitignore rename to perception/autoware_image_projection_based_fusion/.gitignore diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/autoware_image_projection_based_fusion/CMakeLists.txt similarity index 91% rename from perception/image_projection_based_fusion/CMakeLists.txt rename to perception/autoware_image_projection_based_fusion/CMakeLists.txt index 6be7defe422c3..73d305d2ab2a8 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/autoware_image_projection_based_fusion/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(image_projection_based_fusion) +project(autoware_image_projection_based_fusion) # add_compile_options(-Wno-unknown-pragmas) add_compile_options(-Wno-unknown-pragmas -fopenmp) @@ -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/README.md b/perception/autoware_image_projection_based_fusion/README.md similarity index 88% rename from perception/image_projection_based_fusion/README.md rename to perception/autoware_image_projection_based_fusion/README.md index e83cf92400a60..c976697b0396d 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/autoware_image_projection_based_fusion/README.md @@ -1,8 +1,8 @@ -# image_projection_based_fusion +# autoware_image_projection_based_fusion ## Purpose -The `image_projection_based_fusion` is a package to fuse detected obstacles (bounding box or segmentation) from image and 3d pointcloud or obstacles (bounding box, cluster or segmentation). +The `autoware_image_projection_based_fusion` is a package to fuse detected obstacles (bounding box or segmentation) from image and 3d pointcloud or obstacles (bounding box, cluster or segmentation). ## Inner-workings / Algorithms @@ -59,7 +59,7 @@ The `pointpainting_fusion` node has `build_only` option to build the TensorRT en Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: ```bash -ros2 launch image_projection_based_fusion pointpainting_fusion.launch.xml model_name:=pointpainting model_path:=/home/autoware/autoware_data/image_projection_based_fusion model_param_path:=$(ros2 pkg prefix image_projection_based_fusion --share)/config/pointpainting.param.yaml build_only:=true +ros2 launch autoware_image_projection_based_fusion pointpainting_fusion.launch.xml model_name:=pointpainting model_path:=/home/autoware/autoware_data/image_projection_based_fusion model_param_path:=$(ros2 pkg prefix autoware_image_projection_based_fusion --share)/config/pointpainting.param.yaml build_only:=true ``` #### Known Limits diff --git a/perception/image_projection_based_fusion/config/fusion_common.param.yaml b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/fusion_common.param.yaml rename to perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/autoware_image_projection_based_fusion/config/pointpainting.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/pointpainting.param.yaml rename to perception/autoware_image_projection_based_fusion/config/pointpainting.param.yaml diff --git a/perception/image_projection_based_fusion/config/pointpainting_ml_package.param.yaml b/perception/autoware_image_projection_based_fusion/config/pointpainting_ml_package.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/pointpainting_ml_package.param.yaml rename to perception/autoware_image_projection_based_fusion/config/pointpainting_ml_package.param.yaml diff --git a/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml b/perception/autoware_image_projection_based_fusion/config/roi_cluster_fusion.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml rename to perception/autoware_image_projection_based_fusion/config/roi_cluster_fusion.param.yaml diff --git a/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml b/perception/autoware_image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml rename to perception/autoware_image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml diff --git a/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml b/perception/autoware_image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml rename to perception/autoware_image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml similarity index 100% rename from perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml rename to perception/autoware_image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml diff --git a/perception/image_projection_based_fusion/docs/images/pointpainting_fusion.jpg b/perception/autoware_image_projection_based_fusion/docs/images/pointpainting_fusion.jpg similarity index 100% rename from perception/image_projection_based_fusion/docs/images/pointpainting_fusion.jpg rename to perception/autoware_image_projection_based_fusion/docs/images/pointpainting_fusion.jpg diff --git a/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion.png b/perception/autoware_image_projection_based_fusion/docs/images/roi_cluster_fusion.png similarity index 100% rename from perception/image_projection_based_fusion/docs/images/roi_cluster_fusion.png rename to perception/autoware_image_projection_based_fusion/docs/images/roi_cluster_fusion.png diff --git a/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg b/perception/autoware_image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg similarity index 100% rename from perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg rename to perception/autoware_image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg diff --git a/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png b/perception/autoware_image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png similarity index 100% rename from perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png rename to perception/autoware_image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png diff --git a/perception/image_projection_based_fusion/docs/images/roi_sync_1.png b/perception/autoware_image_projection_based_fusion/docs/images/roi_sync_1.png similarity index 100% rename from perception/image_projection_based_fusion/docs/images/roi_sync_1.png rename to perception/autoware_image_projection_based_fusion/docs/images/roi_sync_1.png diff --git a/perception/image_projection_based_fusion/docs/images/roi_sync_2.png b/perception/autoware_image_projection_based_fusion/docs/images/roi_sync_2.png similarity index 100% rename from perception/image_projection_based_fusion/docs/images/roi_sync_2.png rename to perception/autoware_image_projection_based_fusion/docs/images/roi_sync_2.png diff --git a/perception/image_projection_based_fusion/docs/images/segmentation_pointcloud_fusion.png b/perception/autoware_image_projection_based_fusion/docs/images/segmentation_pointcloud_fusion.png similarity index 100% rename from perception/image_projection_based_fusion/docs/images/segmentation_pointcloud_fusion.png rename to perception/autoware_image_projection_based_fusion/docs/images/segmentation_pointcloud_fusion.png diff --git a/perception/image_projection_based_fusion/docs/pointpainting-fusion.md b/perception/autoware_image_projection_based_fusion/docs/pointpainting-fusion.md similarity index 100% rename from perception/image_projection_based_fusion/docs/pointpainting-fusion.md rename to perception/autoware_image_projection_based_fusion/docs/pointpainting-fusion.md diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/autoware_image_projection_based_fusion/docs/roi-cluster-fusion.md similarity index 100% rename from perception/image_projection_based_fusion/docs/roi-cluster-fusion.md rename to perception/autoware_image_projection_based_fusion/docs/roi-cluster-fusion.md diff --git a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md b/perception/autoware_image_projection_based_fusion/docs/roi-detected-object-fusion.md similarity index 100% rename from perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md rename to perception/autoware_image_projection_based_fusion/docs/roi-detected-object-fusion.md diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/autoware_image_projection_based_fusion/docs/roi-pointcloud-fusion.md similarity index 100% rename from perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md rename to perception/autoware_image_projection_based_fusion/docs/roi-pointcloud-fusion.md diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md similarity index 96% rename from perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md rename to perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md index 3c469ac15c6e7..ec2bffca58462 100644 --- a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md +++ b/perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -32,7 +32,7 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud ### Core Parameters -{{ json_to_markdown("perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }} +{{ json_to_markdown("perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }} ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp similarity index 69% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp rename to perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 756cb4224bc20..aa3e3fe096306 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -12,24 +12,24 @@ // 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 "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -71,12 +71,12 @@ class PointPaintingFusionNode bool has_variance_{false}; bool has_twist_{false}; - centerpoint::NonMaximumSuppression iou_bev_nms_; - centerpoint::DetectionClassRemapper detection_class_remapper_; + autoware::lidar_centerpoint::NonMaximumSuppression iou_bev_nms_; + autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; 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/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp similarity index 73% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp rename to perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp index 03609eb18e689..35cfe48d15343 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp +++ b/perception/autoware_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 @@ -23,13 +23,13 @@ #include #endif -#include +#include #include #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { struct PointCloudWithTransform { @@ -40,7 +40,7 @@ struct PointCloudWithTransform class PointCloudDensification { public: - explicit PointCloudDensification(const centerpoint::DensificationParam & param); + explicit PointCloudDensification(const autoware::lidar_centerpoint::DensificationParam & param); bool enqueuePointCloud( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); @@ -61,12 +61,13 @@ class PointCloudDensification void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); void dequeue(); - centerpoint::DensificationParam param_; + autoware::lidar_centerpoint::DensificationParam param_; double current_timestamp_{0.0}; Eigen::Affine3f affine_world2current_; 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/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp new file mode 100644 index 0000000000000..a89ee3d02cd8b --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -0,0 +1,46 @@ +// Copyright 2022 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__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ + +#include +#include + +#include +#include + +namespace autoware::image_projection_based_fusion +{ +class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT +{ +public: + using autoware::lidar_centerpoint::CenterPointTRT::CenterPointTRT; + + explicit PointPaintingTRT( + const autoware::lidar_centerpoint::NetworkParam & encoder_param, + const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::lidar_centerpoint::DensificationParam & densification_param, + const autoware::lidar_centerpoint::CenterPointConfig & config); + +protected: + bool preprocess( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + const tf2_ros::Buffer & tf_buffer) override; + + std::unique_ptr vg_ptr_pp_{nullptr}; +}; +} // namespace autoware::image_projection_based_fusion + +#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/autoware_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/autoware_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/autoware_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/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp similarity index 57% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp rename to perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp index 4cdca8e49ac7e..e314ea0aed5e7 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -12,25 +12,26 @@ // 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 +#include +#include #include #include #include -namespace image_projection_based_fusion +namespace autoware::image_projection_based_fusion { class VoxelGenerator { public: explicit VoxelGenerator( - const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config); + const autoware::lidar_centerpoint::DensificationParam & param, + const autoware::lidar_centerpoint::CenterPointConfig & config); bool enqueuePointCloud( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); @@ -40,11 +41,11 @@ class VoxelGenerator protected: std::unique_ptr pd_ptr_{nullptr}; - centerpoint::CenterPointConfig config_; + autoware::lidar_centerpoint::CenterPointConfig config_; std::array range_; 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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_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/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp similarity index 87% rename from perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp rename to perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 107e0d6c179a8..cfbb2fa4a18cb 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_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; @@ -80,7 +80,7 @@ void closest_cluster( void updateOutputFusedObjects( std::vector & output_objs, std::vector & clusters, - const std::vector clusters_data_size, const PointCloud2 & in_cloud, + const std::vector & clusters_data_size, const PointCloud2 & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects); @@ -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/autoware_image_projection_based_fusion/launch/pointpainting_fusion.launch.xml similarity index 92% rename from perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml rename to perception/autoware_image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index d2f803f13d376..9053237127394 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -21,10 +21,10 @@ - + - + @@ -45,7 +45,7 @@ - + @@ -87,7 +87,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/autoware_image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml similarity index 89% rename from perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml rename to perception/autoware_image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index f624b099fccb3..2a0a7097fcaaa 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -18,8 +18,8 @@ - - + + @@ -31,7 +31,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/autoware_image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml similarity index 89% rename from perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml rename to perception/autoware_image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index f11280b7f7c67..3a2a74e84ec1e 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -18,8 +18,8 @@ - - + + @@ -35,7 +35,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml similarity index 90% rename from perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml rename to perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index cde06744aca58..c062081fb691c 100644 --- a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -20,8 +20,8 @@ - - + + @@ -34,7 +34,7 @@ - + diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/autoware_image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml similarity index 90% rename from perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml rename to perception/autoware_image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml index cf4d104b9e05a..b2716dd4d3ae9 100644 --- a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -18,8 +18,8 @@ - - + + @@ -29,7 +29,7 @@ - + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml similarity index 85% rename from perception/image_projection_based_fusion/package.xml rename to perception/autoware_image_projection_based_fusion/package.xml index 00ffdc8fd5528..74ab1b3454899 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -1,9 +1,9 @@ - image_projection_based_fusion + autoware_image_projection_based_fusion 0.1.0 - The image_projection_based_fusion package + The autoware_image_projection_based_fusion package Yukihiro Saito Shunsuke Miura Yoshi Ri @@ -16,22 +16,19 @@ ament_cmake_auto autoware_cmake + autoware_euclidean_cluster + autoware_lidar_centerpoint autoware_perception_msgs autoware_point_types autoware_universe_utils cv_bridge - euclidean_cluster image_geometry image_transport - 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/schema/fusion_common.schema.json b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json similarity index 100% rename from perception/image_projection_based_fusion/schema/fusion_common.schema.json rename to perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json diff --git a/perception/image_projection_based_fusion/schema/pointpainting.schema.json b/perception/autoware_image_projection_based_fusion/schema/pointpainting.schema.json similarity index 100% rename from perception/image_projection_based_fusion/schema/pointpainting.schema.json rename to perception/autoware_image_projection_based_fusion/schema/pointpainting.schema.json diff --git a/perception/image_projection_based_fusion/schema/pointpainting_ml_package.schema.json b/perception/autoware_image_projection_based_fusion/schema/pointpainting_ml_package.schema.json similarity index 100% rename from perception/image_projection_based_fusion/schema/pointpainting_ml_package.schema.json rename to perception/autoware_image_projection_based_fusion/schema/pointpainting_ml_package.schema.json diff --git a/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json b/perception/autoware_image_projection_based_fusion/schema/roi_cluster_fusion.schema.json similarity index 100% rename from perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json rename to perception/autoware_image_projection_based_fusion/schema/roi_cluster_fusion.schema.json diff --git a/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json b/perception/autoware_image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json similarity index 100% rename from perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json rename to perception/autoware_image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json diff --git a/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json b/perception/autoware_image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json similarity index 100% rename from perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json rename to perception/autoware_image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json diff --git a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json similarity index 100% rename from perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json rename to perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/autoware_image_projection_based_fusion/src/debugger.cpp similarity index 97% rename from perception/image_projection_based_fusion/src/debugger.cpp rename to perception/autoware_image_projection_based_fusion/src/debugger.cpp index a51c23a77aac6..90952f3f6a8f5 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/autoware_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/autoware_image_projection_based_fusion/src/fusion_node.cpp similarity index 99% rename from perception/image_projection_based_fusion/src/fusion_node.cpp rename to perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 20273fd1de547..6cc4a0c23adcc 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_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/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp similarity index 93% rename from perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp rename to perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 6947331d6bcfa..be14d9a0b0fb3 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -12,18 +12,18 @@ // 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 -#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) @@ -167,8 +167,8 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); { - centerpoint::NMSParams p; - p.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; + autoware::lidar_centerpoint::NMSParams p; + p.nms_type_ = autoware::lidar_centerpoint::NMS_TYPE::IoU_BEV; p.target_class_names_ = this->declare_parameter>( "post_process_params.iou_nms_target_class_names"); p.search_distance_2d_ = @@ -177,11 +177,13 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt iou_bev_nms_.setParameters(p); } - centerpoint::NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); - centerpoint::NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); - centerpoint::DensificationParam densification_param( + autoware::lidar_centerpoint::NetworkParam encoder_param( + encoder_onnx_path, encoder_engine_path, trt_precision); + autoware::lidar_centerpoint::NetworkParam head_param( + head_onnx_path, head_engine_path, trt_precision); + autoware::lidar_centerpoint::DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); - centerpoint::CenterPointConfig config( + autoware::lidar_centerpoint::CenterPointConfig config( 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_); @@ -389,7 +391,7 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte return; } - std::vector det_boxes3d; + std::vector det_boxes3d; bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { return; @@ -418,7 +420,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/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp similarity index 89% rename from perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp rename to perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp index 9edf6a73cd59b..390d92abae3b4 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp +++ b/perception/autoware_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,9 +54,10 @@ 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) +PointCloudDensification::PointCloudDensification( + const autoware::lidar_centerpoint::DensificationParam & param) : param_(param) { } @@ -100,4 +101,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/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp similarity index 80% rename from perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp rename to perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index c1e431ed83c99..cc3eb17cc32ab 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -12,25 +12,27 @@ // 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 -#include -#include #include #include #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, - const centerpoint::DensificationParam & densification_param, - const centerpoint::CenterPointConfig & config) -: centerpoint::CenterPointTRT(encoder_param, head_param, densification_param, config) + const autoware::lidar_centerpoint::NetworkParam & encoder_param, + const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::lidar_centerpoint::DensificationParam & densification_param, + const autoware::lidar_centerpoint::CenterPointConfig & config) +: autoware::lidar_centerpoint::CenterPointTRT( + encoder_param, head_param, densification_param, config) { vg_ptr_pp_ = std::make_unique(densification_param, config_); @@ -81,4 +83,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/autoware_image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu similarity index 97% rename from perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu rename to perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index 68e08ac61a569..91804f0df8665 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -28,10 +28,10 @@ * 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 +// #include namespace { @@ -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/autoware_image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp old mode 100755 new mode 100644 similarity index 90% rename from perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp rename to perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index cb3fc33d3e022..205d231ab32bd --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -12,15 +12,16 @@ // 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( - const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config) + const autoware::lidar_centerpoint::DensificationParam & param, + const autoware::lidar_centerpoint::CenterPointConfig & config) : config_(config) { pd_ptr_ = std::make_unique(param); @@ -83,4 +84,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/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp similarity index 96% rename from perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp rename to perception/autoware_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/autoware_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/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp similarity index 96% rename from perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp rename to perception/autoware_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/autoware_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/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp similarity index 94% rename from perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp rename to perception/autoware_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/autoware_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/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp similarity index 93% rename from perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp rename to perception/autoware_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/autoware_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/autoware_image_projection_based_fusion/src/utils/geometry.cpp similarity index 97% rename from perception/image_projection_based_fusion/src/utils/geometry.cpp rename to perception/autoware_image_projection_based_fusion/src/utils/geometry.cpp index 29198280ec7b2..4bea6ac0fe713 100644 --- a/perception/image_projection_based_fusion/src/utils/geometry.cpp +++ b/perception/autoware_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/autoware_image_projection_based_fusion/src/utils/utils.cpp similarity index 97% rename from perception/image_projection_based_fusion/src/utils/utils.cpp rename to perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index aeec2886a801a..ac17c419efca4 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_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) @@ -118,7 +118,7 @@ void closest_cluster( void updateOutputFusedObjects( std::vector & output_objs, std::vector & clusters, - const std::vector clusters_data_size, const PointCloud2 & in_cloud, + const std::vector & clusters_data_size, const PointCloud2 & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects) @@ -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/autoware_image_projection_based_fusion/test/test_calc_iou_functions.cpp similarity index 91% rename from perception/image_projection_based_fusion/test/test_calc_iou_functions.cpp rename to perception/autoware_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/autoware_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/autoware_image_projection_based_fusion/test/test_geometry.cpp similarity index 84% rename from perception/image_projection_based_fusion/test/test_geometry.cpp rename to perception/autoware_image_projection_based_fusion/test/test_geometry.cpp index eae5f7cef728c..cf3778e7a0c84 100644 --- a/perception/image_projection_based_fusion/test/test_geometry.cpp +++ b/perception/autoware_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/autoware_image_projection_based_fusion/test/test_pointpainting_fusion.cpp similarity index 76% rename from perception/image_projection_based_fusion/test/test_pointpainting_fusion.cpp rename to perception/autoware_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/autoware_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/autoware_image_projection_based_fusion/test/test_utils.cpp similarity index 94% rename from perception/image_projection_based_fusion/test/test_utils.cpp rename to perception/autoware_image_projection_based_fusion/test/test_utils.cpp index e578ce1987cc3..5fbd313d75dfe 100644 --- a/perception/image_projection_based_fusion/test/test_utils.cpp +++ b/perception/autoware_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_apollo_instance_segmentation/.gitignore b/perception/autoware_lidar_apollo_instance_segmentation/.gitignore similarity index 100% rename from perception/lidar_apollo_instance_segmentation/.gitignore rename to perception/autoware_lidar_apollo_instance_segmentation/.gitignore diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt similarity index 93% rename from perception/lidar_apollo_instance_segmentation/CMakeLists.txt rename to perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt index 462e9d22f9da2..b08fd94b192e2 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.17) -project(lidar_apollo_instance_segmentation) +project(autoware_lidar_apollo_instance_segmentation) find_package(ament_cmake REQUIRED) find_package(autoware_perception_msgs REQUIRED) @@ -74,7 +74,7 @@ install(TARGETS ${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "lidar_apollo_instance_segmentation::LidarInstanceSegmentationNode" + PLUGIN "autoware::lidar_apollo_instance_segmentation::LidarInstanceSegmentationNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/perception/lidar_apollo_instance_segmentation/README.md b/perception/autoware_lidar_apollo_instance_segmentation/README.md similarity index 99% rename from perception/lidar_apollo_instance_segmentation/README.md rename to perception/autoware_lidar_apollo_instance_segmentation/README.md index f9dc6b3f44a23..edc3d99e0967f 100644 --- a/perception/lidar_apollo_instance_segmentation/README.md +++ b/perception/autoware_lidar_apollo_instance_segmentation/README.md @@ -1,4 +1,4 @@ -# lidar_apollo_instance_segmentation +# autoware_lidar_apollo_instance_segmentation ![Peek 2020-04-07 00-17](https://user-images.githubusercontent.com/8327598/78574862-92507d80-7865-11ea-9a2d-56d3453bdb7a.gif) diff --git a/perception/lidar_apollo_instance_segmentation/config/hdl-64.param.yaml b/perception/autoware_lidar_apollo_instance_segmentation/config/hdl-64.param.yaml similarity index 100% rename from perception/lidar_apollo_instance_segmentation/config/hdl-64.param.yaml rename to perception/autoware_lidar_apollo_instance_segmentation/config/hdl-64.param.yaml diff --git a/perception/lidar_apollo_instance_segmentation/config/vlp-16.param.yaml b/perception/autoware_lidar_apollo_instance_segmentation/config/vlp-16.param.yaml similarity index 100% rename from perception/lidar_apollo_instance_segmentation/config/vlp-16.param.yaml rename to perception/autoware_lidar_apollo_instance_segmentation/config/vlp-16.param.yaml diff --git a/perception/lidar_apollo_instance_segmentation/config/vls-128.param.yaml b/perception/autoware_lidar_apollo_instance_segmentation/config/vls-128.param.yaml similarity index 100% rename from perception/lidar_apollo_instance_segmentation/config/vls-128.param.yaml rename to perception/autoware_lidar_apollo_instance_segmentation/config/vls-128.param.yaml diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/cluster2d.hpp similarity index 95% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/cluster2d.hpp index 1bb6da0374fe9..0c9bb61accae7 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/cluster2d.hpp @@ -43,8 +43,8 @@ * limitations under the License. *****************************************************************************/ -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ #include "disjoint_set.hpp" #include "util.hpp" @@ -60,6 +60,8 @@ #include #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { enum MetaType { @@ -161,5 +163,6 @@ class Cluster2D void traverse(Node * x); }; } // namespace lidar_apollo_instance_segmentation +} // namespace autoware -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/debugger.hpp similarity index 81% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/debugger.hpp index 954d7c5cd1582..90f49b36dca5b 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/debugger.hpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ #include #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { class Debugger @@ -33,5 +35,6 @@ class Debugger rclcpp::Publisher::SharedPtr instance_pointcloud_pub_; }; } // namespace lidar_apollo_instance_segmentation +} // namespace autoware -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DEBUGGER_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/detector.hpp similarity index 82% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/detector.hpp index 1e5966f5751af..053cdcfdd8d76 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/detector.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ -#include "cluster2d.hpp" -#include "feature_generator.hpp" -#include "lidar_apollo_instance_segmentation/node.hpp" +#include "autoware/lidar_apollo_instance_segmentation/cluster2d.hpp" +#include "autoware/lidar_apollo_instance_segmentation/feature_generator.hpp" +#include "autoware/lidar_apollo_instance_segmentation/node.hpp" #include #include @@ -31,6 +31,8 @@ #include #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { using cuda_utils::CudaUniquePtr; @@ -72,5 +74,6 @@ class LidarApolloInstanceSegmentation : public LidarInstanceSegmentationInterfac StreamUniquePtr stream_{makeCudaStream()}; }; } // namespace lidar_apollo_instance_segmentation +} // namespace autoware -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DETECTOR_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/disjoint_set.hpp similarity index 87% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/disjoint_set.hpp index 023dd3b1c3515..0170a171f256f 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/disjoint_set.hpp @@ -14,9 +14,11 @@ * limitations under the License. *****************************************************************************/ -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ +namespace autoware +{ namespace lidar_apollo_instance_segmentation { template @@ -74,5 +76,5 @@ void DisjointSetUnion(T * x, T * y) } } } // namespace lidar_apollo_instance_segmentation - -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ +} // namespace autoware +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/feature_generator.hpp similarity index 78% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/feature_generator.hpp index 13ee9647cbea4..d8e32bac04ea8 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/feature_generator.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ -#include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include "autoware/lidar_apollo_instance_segmentation/feature_map.hpp" #include "util.hpp" #include @@ -23,6 +23,8 @@ #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { class FeatureGenerator @@ -44,5 +46,5 @@ class FeatureGenerator const pcl::PointCloud::Ptr & pc_ptr); }; } // namespace lidar_apollo_instance_segmentation - -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ +} // namespace autoware +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_GENERATOR_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/feature_map.hpp similarity index 90% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/feature_map.hpp index 5cbfbd16c1c3a..697f7c3af4701 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/feature_map.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ #include #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { struct FeatureMapInterface @@ -69,5 +71,5 @@ struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface void resetMap(std::vector & map) override; }; } // namespace lidar_apollo_instance_segmentation - -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ +} // namespace autoware +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__FEATURE_MAP_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/log_table.hpp similarity index 74% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/log_table.hpp index b1aa49d579928..c3ba0ace7ec6d 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/log_table.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ +namespace autoware +{ namespace lidar_apollo_instance_segmentation { float calcApproximateLog(float num); } // namespace lidar_apollo_instance_segmentation - -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ +} // namespace autoware +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__LOG_TABLE_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/node.hpp similarity index 86% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/node.hpp index f159f125677c4..c83b18d426325 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ -#include "lidar_apollo_instance_segmentation/debugger.hpp" +#include "autoware/lidar_apollo_instance_segmentation/debugger.hpp" #include #include @@ -26,6 +26,8 @@ #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { class LidarInstanceSegmentationInterface @@ -54,5 +56,5 @@ class LidarInstanceSegmentationNode : public rclcpp::Node explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); }; } // namespace lidar_apollo_instance_segmentation - -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ +} // namespace autoware +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__NODE_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/util.hpp similarity index 90% rename from perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp rename to perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/util.hpp index 8837919436dbe..52ec6bb807c39 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/include/autoware/lidar_apollo_instance_segmentation/util.hpp @@ -28,12 +28,13 @@ * limitations under the License. *****************************************************************************/ -#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_ -#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_ +#ifndef AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_ +#define AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_ #include -#include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { // project point cloud to 2d map. calc in which grid point is. @@ -58,5 +59,5 @@ inline float Pixel2Pc(int in_pixel, float in_size, float out_range) return out_range - (static_cast(in_pixel) + 0.5f) * res; } } // namespace lidar_apollo_instance_segmentation - -#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_ +} // namespace autoware +#endif // AUTOWARE__LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/autoware_lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml similarity index 82% rename from perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml rename to perception/autoware_lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index 207dac45671dd..4defc72442dcc 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/autoware_lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -10,14 +10,14 @@ - + - + diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/autoware_lidar_apollo_instance_segmentation/package.xml similarity index 89% rename from perception/lidar_apollo_instance_segmentation/package.xml rename to perception/autoware_lidar_apollo_instance_segmentation/package.xml index cf6f5a514aa0d..57bcc9c6881c6 100755 --- a/perception/lidar_apollo_instance_segmentation/package.xml +++ b/perception/autoware_lidar_apollo_instance_segmentation/package.xml @@ -1,9 +1,9 @@ - lidar_apollo_instance_segmentation + autoware_lidar_apollo_instance_segmentation 0.1.0 - lidar_apollo_instance_segmentation + autoware_lidar_apollo_instance_segmentation Yoshi Ri Yukihiro Saito Apache License 2.0 diff --git a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/cluster2d.cpp similarity index 98% rename from perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp rename to perception/autoware_lidar_apollo_instance_segmentation/src/cluster2d.cpp index 94da6e3959330..0450eca4e3759 100644 --- a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/cluster2d.cpp @@ -44,7 +44,7 @@ * limitations under the License. *****************************************************************************/ -#include "lidar_apollo_instance_segmentation/cluster2d.hpp" +#include "autoware/lidar_apollo_instance_segmentation/cluster2d.hpp" #include #include @@ -54,8 +54,15 @@ #include #else #include + +#include +#include +#include + #endif +namespace autoware +{ namespace lidar_apollo_instance_segmentation { geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y) @@ -377,3 +384,4 @@ void Cluster2D::getObjects( objects.header = in_header; } } // namespace lidar_apollo_instance_segmentation +} // namespace autoware diff --git a/perception/lidar_apollo_instance_segmentation/src/debugger.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/debugger.cpp similarity index 84% rename from perception/lidar_apollo_instance_segmentation/src/debugger.cpp rename to perception/autoware_lidar_apollo_instance_segmentation/src/debugger.cpp index 8e12ff75ae7c3..3a76f92f9a5e6 100644 --- a/perception/lidar_apollo_instance_segmentation/src/debugger.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/debugger.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_apollo_instance_segmentation/debugger.hpp" +#include "autoware/lidar_apollo_instance_segmentation/debugger.hpp" #include #include @@ -21,6 +21,8 @@ #include #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { Debugger::Debugger(rclcpp::Node * node) @@ -34,12 +36,12 @@ void Debugger::publishColoredPointCloud( { using autoware_perception_msgs::msg::ObjectClassification; pcl::PointCloud colored_pointcloud; - for (size_t i = 0; i < input.feature_objects.size(); i++) { + for (const auto & feature_object : input.feature_objects) { pcl::PointCloud object_pointcloud; - pcl::fromROSMsg(input.feature_objects.at(i).feature.cluster, object_pointcloud); + pcl::fromROSMsg(feature_object.feature.cluster, object_pointcloud); int red = 0, green = 0, blue = 0; - switch (input.feature_objects.at(i).object.classification.front().label) { + switch (feature_object.object.classification.front().label) { case ObjectClassification::CAR: { red = 255; green = 0; @@ -84,11 +86,11 @@ void Debugger::publishColoredPointCloud( } } - for (size_t i = 0; i < object_pointcloud.size(); i++) { + for (const auto & point : object_pointcloud) { pcl::PointXYZRGB colored_point; - colored_point.x = object_pointcloud[i].x; - colored_point.y = object_pointcloud[i].y; - colored_point.z = object_pointcloud[i].z; + colored_point.x = point.x; + colored_point.y = point.y; + colored_point.z = point.z; colored_point.r = red; colored_point.g = green; colored_point.b = blue; @@ -101,3 +103,4 @@ void Debugger::publishColoredPointCloud( instance_pointcloud_pub_->publish(output_msg); } } // namespace lidar_apollo_instance_segmentation +} // namespace autoware diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp similarity index 94% rename from perception/lidar_apollo_instance_segmentation/src/detector.cpp rename to perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp index ba4d7f788b0f5..d2cad343ffc41 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_apollo_instance_segmentation/detector.hpp" +#include "autoware/lidar_apollo_instance_segmentation/detector.hpp" -#include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include "autoware/lidar_apollo_instance_segmentation/feature_map.hpp" #include @@ -22,6 +22,12 @@ #include #include +#include +#include +#include + +namespace autoware +{ namespace lidar_apollo_instance_segmentation { LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * node) @@ -181,13 +187,14 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( score_threshold_, height_thresh, min_pts_num, output, transformed_cloud.header); // move down pointcloud z_offset in z axis - for (std::size_t i = 0; i < output.feature_objects.size(); i++) { - sensor_msgs::msg::PointCloud2 transformed_cloud; - transformCloud(output.feature_objects.at(i).feature.cluster, transformed_cloud, -z_offset_); - output.feature_objects.at(i).feature.cluster = transformed_cloud; + for (auto & feature_object : output.feature_objects) { + sensor_msgs::msg::PointCloud2 updated_cloud; + transformCloud(feature_object.feature.cluster, updated_cloud, -z_offset_); + feature_object.feature.cluster = updated_cloud; } output.header = transformed_cloud.header; return true; } } // namespace lidar_apollo_instance_segmentation +} // namespace autoware diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/feature_generator.cpp similarity index 94% rename from perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp rename to perception/autoware_lidar_apollo_instance_segmentation/src/feature_generator.cpp index 56291e086e41f..e1aa0fc0494de 100644 --- a/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/feature_generator.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_apollo_instance_segmentation/feature_generator.hpp" +#include "autoware/lidar_apollo_instance_segmentation/feature_generator.hpp" -#include "lidar_apollo_instance_segmentation/log_table.hpp" +#include "autoware/lidar_apollo_instance_segmentation/log_table.hpp" + +#include namespace { @@ -24,6 +26,8 @@ inline float normalizeIntensity(float intensity) } } // namespace +namespace autoware +{ namespace lidar_apollo_instance_segmentation { FeatureGenerator::FeatureGenerator( @@ -99,3 +103,4 @@ std::shared_ptr FeatureGenerator::generate( return map_ptr_; } } // namespace lidar_apollo_instance_segmentation +} // namespace autoware diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/feature_map.cpp similarity index 96% rename from perception/lidar_apollo_instance_segmentation/src/feature_map.cpp rename to perception/autoware_lidar_apollo_instance_segmentation/src/feature_map.cpp index 024a62ed2a08e..461e8167776e8 100644 --- a/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/feature_map.cpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include "autoware/lidar_apollo_instance_segmentation/feature_map.hpp" -#include "lidar_apollo_instance_segmentation/util.hpp" +#include "autoware/lidar_apollo_instance_segmentation/util.hpp" #include +#include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { FeatureMapInterface::FeatureMapInterface( @@ -169,3 +172,4 @@ void FeatureMapWithConstantAndIntensity::resetMap([[maybe_unused]] std::vector -#include #include namespace @@ -35,6 +34,8 @@ struct LogTable static ::LogTable log_table; +namespace autoware +{ namespace lidar_apollo_instance_segmentation { float calcApproximateLog(float num) @@ -46,3 +47,4 @@ float calcApproximateLog(float num) return std::log(static_cast(1.0 + num)); } } // namespace lidar_apollo_instance_segmentation +} // namespace autoware diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp similarity index 90% rename from perception/lidar_apollo_instance_segmentation/src/node.cpp rename to perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp index 93b2f3344de99..3314b1c7421d3 100644 --- a/perception/lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_apollo_instance_segmentation/node.hpp" +#include "autoware/lidar_apollo_instance_segmentation/node.hpp" -#include "lidar_apollo_instance_segmentation/detector.hpp" +#include "autoware/lidar_apollo_instance_segmentation/detector.hpp" #include #include +namespace autoware +{ namespace lidar_apollo_instance_segmentation { LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( @@ -65,6 +67,8 @@ void LidarInstanceSegmentationNode::pointCloudCallback( } } } // namespace lidar_apollo_instance_segmentation +} // namespace autoware #include -RCLCPP_COMPONENTS_REGISTER_NODE(lidar_apollo_instance_segmentation::LidarInstanceSegmentationNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::lidar_apollo_instance_segmentation::LidarInstanceSegmentationNode) diff --git a/perception/lidar_centerpoint/.gitignore b/perception/autoware_lidar_centerpoint/.gitignore similarity index 100% rename from perception/lidar_centerpoint/.gitignore rename to perception/autoware_lidar_centerpoint/.gitignore diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/autoware_lidar_centerpoint/CMakeLists.txt similarity index 86% rename from perception/lidar_centerpoint/CMakeLists.txt rename to perception/autoware_lidar_centerpoint/CMakeLists.txt index 915224a50d53f..88244b3153349 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/autoware_lidar_centerpoint/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(lidar_centerpoint) +project(autoware_lidar_centerpoint) find_package(autoware_cmake REQUIRED) autoware_package() -# TODO(lidar_centerpoint): Remove once upgrading to TensorRT 8.5 is complete +# TODO(autoware_lidar_centerpoint): Remove once upgrading to TensorRT 8.5 is complete add_compile_options(-Wno-deprecated-declarations) option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) @@ -79,7 +79,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ### centerpoint ### - ament_auto_add_library(centerpoint_lib SHARED + ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/centerpoint_trt.cpp lib/detection_class_remapper.cpp lib/utils.cpp @@ -91,24 +91,24 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/preprocess/voxel_generator.cpp ) - cuda_add_library(centerpoint_cuda_lib SHARED + cuda_add_library(${PROJECT_NAME}_cuda_lib SHARED lib/postprocess/circle_nms_kernel.cu lib/postprocess/postprocess_kernel.cu lib/network/scatter_kernel.cu lib/preprocess/preprocess_kernel.cu ) - target_link_libraries(centerpoint_lib + target_link_libraries(${PROJECT_NAME}_lib ${NVINFER} ${NVONNXPARSER} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} - centerpoint_cuda_lib + ${PROJECT_NAME}_cuda_lib ) - target_include_directories(centerpoint_lib + target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ @@ -116,23 +116,23 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6. # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe - target_include_directories(centerpoint_lib + target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC ${CUDA_INCLUDE_DIRS} ) ## node ## - ament_auto_add_library(lidar_centerpoint_component SHARED + ament_auto_add_library(${PROJECT_NAME}_component SHARED src/node.cpp ) - target_link_libraries(lidar_centerpoint_component - centerpoint_lib + target_link_libraries(${PROJECT_NAME}_component + ${PROJECT_NAME}_lib ) - rclcpp_components_register_node(lidar_centerpoint_component - PLUGIN "centerpoint::LidarCenterPointNode" - EXECUTABLE lidar_centerpoint_node + rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "autoware::lidar_centerpoint::LidarCenterPointNode" + EXECUTABLE ${PROJECT_NAME}_node ) ament_export_dependencies(ament_cmake_python) @@ -144,7 +144,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) install( - TARGETS centerpoint_cuda_lib + TARGETS ${PROJECT_NAME}_cuda_lib DESTINATION lib ) @@ -176,7 +176,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # ) # # target_link_libraries(test_preprocess_kernel -# centerpoint_cuda_lib +# ${PROJECT_NAME}_cuda_lib # gtest # gtest_main # ) @@ -196,7 +196,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # ) # # target_link_libraries(test_postprocess_kernel -# centerpoint_cuda_lib +# ${PROJECT_NAME}_cuda_lib # gtest # gtest_main # ) diff --git a/perception/lidar_centerpoint/README.md b/perception/autoware_lidar_centerpoint/README.md similarity index 84% rename from perception/lidar_centerpoint/README.md rename to perception/autoware_lidar_centerpoint/README.md index fa42655b6ca1f..471712a637dec 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/autoware_lidar_centerpoint/README.md @@ -1,8 +1,8 @@ -# lidar_centerpoint +# autoware_lidar_centerpoint ## Purpose -lidar_centerpoint is a package for detecting dynamic 3D objects. +autoware_lidar_centerpoint is a package for detecting dynamic 3D objects. ## Inner-workings / Algorithms @@ -28,6 +28,22 @@ We trained the models using . ## Parameters +### ML Model Parameters + +Note that these parameters are associated with ONNX file, predefined during the training phase. Be careful to change ONNX file as well when changing this parameter. Also, whenever you update the ONNX file, do NOT forget to check these values. + +| Name | Type | Default Value | Description | +| -------------------------------------- | ------------ | ------------------------------------------------ | --------------------------------------------------------------------- | +| `model_params.class_names` | list[string] | ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] | list of class names for model outputs | +| `model_params.point_feature_size` | int | `4` | number of features per point in the point cloud | +| `model_params.max_voxel_size` | int | `40000` | maximum number of voxels | +| `model_params.point_cloud_range` | list[double] | [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0] | detection range [min_x, min_y, min_z, max_x, max_y, max_z] [m] | +| `model_params.voxel_size` | list[double] | [0.32, 0.32, 10.0] | size of each voxel [x, y, z] [m] | +| `model_params.downsample_factor` | int | `1` | downsample factor for coordinates | +| `model_params.encoder_in_feature_size` | int | `9` | number of input features to the encoder | +| `model_params.has_variance` | bool | `false` | true if the model outputs pose variance as well as pose for each bbox | +| `model_params.has_twist` | bool | `false` | true if the model outputs velocity as well as pose for each bbox | + ### Core Parameters | Name | Type | Default Value | Description | @@ -49,11 +65,11 @@ We trained the models using . ### The `build_only` option -The `lidar_centerpoint` node has `build_only` option to build the TensorRT engine file from the ONNX file. +The `autoware_lidar_centerpoint` node has `build_only` option to build the TensorRT engine file from the ONNX file. Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: ```bash -ros2 launch lidar_centerpoint lidar_centerpoint.launch.xml model_name:=centerpoint_tiny model_path:=/home/autoware/autoware_data/lidar_centerpoint model_param_path:=$(ros2 pkg prefix lidar_centerpoint --share)/config/centerpoint_tiny.param.yaml build_only:=true +ros2 launch autoware_lidar_centerpoint lidar_centerpoint.launch.xml model_name:=centerpoint_tiny model_path:=/home/autoware/autoware_data/lidar_centerpoint model_param_path:=$(ros2 pkg prefix autoware_lidar_centerpoint --share)/config/centerpoint_tiny.param.yaml build_only:=true ``` ## Assumptions / Known limits @@ -225,7 +241,7 @@ the base link location of the vehicle. #### Convert CenterPoint PyTorch model to ONNX Format -The lidar_centerpoint implementation requires two ONNX models as input the voxel encoder and the backbone-neck-head of the CenterPoint model, other aspects of the network, +The autoware_lidar_centerpoint implementation requires two ONNX models as input the voxel encoder and the backbone-neck-head of the CenterPoint model, other aspects of the network, such as preprocessing operations, are implemented externally. Under the fork of the mmdetection3d repository, we have included a script that converts the CenterPoint model to Autoware compatible ONNX format. You can find it in `mmdetection3d/projects/AutowareCenterPoint` file. @@ -236,7 +252,7 @@ python projects/AutowareCenterPoint/centerpoint_onnx_converter.py --cfg projects #### Create the config file for the custom model -Create a new config file named **centerpoint_custom.param.yaml** under the config file directory of the lidar_centerpoint node. Sets the parameters of the config file like +Create a new config file named **centerpoint_custom.param.yaml** under the config file directory of the autoware_lidar_centerpoint node. Sets the parameters of the config file like point_cloud_range, point_feature_size, voxel_size, etc. according to the training config file. ```yaml @@ -262,7 +278,7 @@ point_cloud_range, point_feature_size, voxel_size, etc. according to the trainin ```bash cd /YOUR/AUTOWARE/PATH/Autoware source install/setup.bash -ros2 launch lidar_centerpoint lidar_centerpoint.launch.xml model_name:=centerpoint_custom model_path:=/PATH/TO/ONNX/FILE/ +ros2 launch autoware_lidar_centerpoint lidar_centerpoint.launch.xml model_name:=centerpoint_custom model_path:=/PATH/TO/ONNX/FILE/ ``` ### Changelog diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/autoware_lidar_centerpoint/config/centerpoint.param.yaml similarity index 100% rename from perception/lidar_centerpoint/config/centerpoint.param.yaml rename to perception/autoware_lidar_centerpoint/config/centerpoint.param.yaml diff --git a/perception/lidar_centerpoint/config/centerpoint_ml_package.param.yaml b/perception/autoware_lidar_centerpoint/config/centerpoint_ml_package.param.yaml similarity index 100% rename from perception/lidar_centerpoint/config/centerpoint_ml_package.param.yaml rename to perception/autoware_lidar_centerpoint/config/centerpoint_ml_package.param.yaml diff --git a/perception/lidar_centerpoint/config/centerpoint_sigma_ml_package.param.yaml b/perception/autoware_lidar_centerpoint/config/centerpoint_sigma_ml_package.param.yaml similarity index 100% rename from perception/lidar_centerpoint/config/centerpoint_sigma_ml_package.param.yaml rename to perception/autoware_lidar_centerpoint/config/centerpoint_sigma_ml_package.param.yaml diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/autoware_lidar_centerpoint/config/centerpoint_tiny.param.yaml similarity index 100% rename from perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml rename to perception/autoware_lidar_centerpoint/config/centerpoint_tiny.param.yaml diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny_ml_package.param.yaml b/perception/autoware_lidar_centerpoint/config/centerpoint_tiny_ml_package.param.yaml similarity index 100% rename from perception/lidar_centerpoint/config/centerpoint_tiny_ml_package.param.yaml rename to perception/autoware_lidar_centerpoint/config/centerpoint_tiny_ml_package.param.yaml diff --git a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml b/perception/autoware_lidar_centerpoint/config/detection_class_remapper.param.yaml similarity index 100% rename from perception/lidar_centerpoint/config/detection_class_remapper.param.yaml rename to perception/autoware_lidar_centerpoint/config/detection_class_remapper.param.yaml diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_config.hpp similarity index 94% rename from perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_config.hpp index 7b560cf46e2e3..57ed124ab789f 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_config.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_ -#define LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class CenterPointConfig { @@ -132,6 +132,6 @@ class CenterPointConfig std::size_t down_grid_size_y_ = grid_size_y_ / downsample_factor_; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_CONFIG_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp similarity index 86% rename from perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp index 52ae86951c7cf..f622fe18c6845 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ -#define LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ -#include "lidar_centerpoint/cuda_utils.hpp" -#include "lidar_centerpoint/network/network_trt.hpp" -#include "lidar_centerpoint/postprocess/postprocess_kernel.hpp" -#include "lidar_centerpoint/preprocess/voxel_generator.hpp" +#include "autoware/lidar_centerpoint/cuda_utils.hpp" +#include "autoware/lidar_centerpoint/network/network_trt.hpp" +#include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp" +#include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp" #include "pcl/point_cloud.h" #include "pcl/point_types.h" @@ -29,7 +29,7 @@ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class NetworkParam { @@ -107,6 +107,6 @@ class CenterPointTRT cuda::unique_ptr num_voxels_d_{nullptr}; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/cuda_utils.hpp similarity index 95% rename from perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/cuda_utils.hpp index efe3bbb9a5482..dcb264e672a16 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/cuda_utils.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/cuda_utils.hpp @@ -39,8 +39,8 @@ * https://creativecommons.org/publicdomain/zero/1.0/deed.en */ -#ifndef LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ -#define LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ #include "cuda_runtime_api.h" @@ -117,4 +117,4 @@ inline T * get_next_ptr(size_t num_elem, void *& workspace, size_t & workspace_s } // namespace cuda -#endif // LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__CUDA_UTILS_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/detection_class_remapper.hpp similarity index 82% rename from perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/detection_class_remapper.hpp index d961dd998af76..6748444c05a96 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/detection_class_remapper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ -#define LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ #include @@ -24,7 +24,7 @@ #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class DetectionClassRemapper @@ -42,6 +42,6 @@ class DetectionClassRemapper int num_labels_; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp similarity index 75% rename from perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp index 1bf77248efe08..17778d77ed798 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ -#define LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ -#include "lidar_centerpoint/centerpoint_config.hpp" -#include "lidar_centerpoint/network/tensorrt_wrapper.hpp" +#include "autoware/lidar_centerpoint/centerpoint_config.hpp" +#include "autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp" #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class VoxelEncoderTRT : public TensorRTWrapper { @@ -48,6 +48,6 @@ class HeadTRT : public TensorRTWrapper std::vector out_channel_sizes_; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/scatter_kernel.hpp similarity index 77% rename from perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/scatter_kernel.hpp index ad5e222ce01e8..d943d704b75fd 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/scatter_kernel.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ -#define LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ #include "cuda.h" #include "cuda_runtime_api.h" -namespace centerpoint +namespace autoware::lidar_centerpoint { cudaError_t scatterFeatures_launch( const float * pillar_features, const int * coords, const unsigned int * num_pillars, @@ -26,6 +26,6 @@ cudaError_t scatterFeatures_launch( const std::size_t grid_size_x, const std::size_t grid_size_y, float * scattered_features, cudaStream_t stream); -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp similarity index 83% rename from perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp index 4af5aac7ff7fe..1d731b5d819d7 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ -#define LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ #include "NvInfer.h" -#include "lidar_centerpoint/centerpoint_config.hpp" +#include "autoware/lidar_centerpoint/centerpoint_config.hpp" #include "tensorrt_common/tensorrt_common.hpp" #include #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class TensorRTWrapper @@ -62,6 +62,6 @@ class TensorRTWrapper tensorrt_common::TrtUniquePtr engine_{nullptr}; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp similarity index 84% rename from perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index 154ff97cdb887..eb056ed14c57f 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__NODE_HPP_ -#define LIDAR_CENTERPOINT__NODE_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__NODE_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__NODE_HPP_ -#include "lidar_centerpoint/centerpoint_trt.hpp" -#include "lidar_centerpoint/detection_class_remapper.hpp" -#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_centerpoint/centerpoint_trt.hpp" +#include "autoware/lidar_centerpoint/detection_class_remapper.hpp" +#include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include #include @@ -34,7 +34,7 @@ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class LidarCenterPointNode : public rclcpp::Node @@ -69,6 +69,6 @@ class LidarCenterPointNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__NODE_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__NODE_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp similarity index 72% rename from perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp index 5b55636a3eec4..463e97c2a8e4e 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/circle_nms_kernel.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#define LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#include "lidar_centerpoint/utils.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" #include "thrust/device_vector.h" -namespace centerpoint +namespace autoware::lidar_centerpoint { // Non-maximum suppression (NMS) uses the distance on the xy plane instead of // intersection over union (IoU) to suppress overlapped objects. @@ -26,6 +26,6 @@ std::size_t circleNMS( thrust::device_vector & boxes3d, const float distance_threshold, thrust::device_vector & keep_mask, cudaStream_t stream); -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp similarity index 83% rename from perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp index 3cefe7ddf3335..57abd053af718 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ -#define LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ -#include "lidar_centerpoint/ros_utils.hpp" +#include "autoware/lidar_centerpoint/ros_utils.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { using autoware_perception_msgs::msg::DetectedObject; @@ -76,6 +76,6 @@ class NonMaximumSuppression std::vector target_class_mask_{}; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp similarity index 74% rename from perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp index e15d3022c947c..4792fed3e9362 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ -#define LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#include "autoware/lidar_centerpoint/centerpoint_config.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" #include "cuda.h" #include "cuda_runtime_api.h" -#include "lidar_centerpoint/centerpoint_config.hpp" -#include "lidar_centerpoint/utils.hpp" #include "thrust/device_vector.h" #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class PostProcessCUDA { @@ -41,6 +41,6 @@ class PostProcessCUDA thrust::device_vector yaw_norm_thresholds_d_; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp similarity index 87% rename from perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp index c1d6a6b060955..67c0d34a0e729 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/pointcloud_densification.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#define LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -27,9 +27,9 @@ #include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" #endif -#include "lidar_centerpoint/cuda_utils.hpp" +#include "autoware/lidar_centerpoint/cuda_utils.hpp" -namespace centerpoint +namespace autoware::lidar_centerpoint { class DensificationParam { @@ -89,6 +89,6 @@ class PointCloudDensification std::list pointcloud_cache_; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp similarity index 86% rename from perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp index 3abcd89cb5c55..5a39f4e8d1103 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ -#define LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ #include "cuda.h" #include "cuda_runtime_api.h" -namespace centerpoint +namespace autoware::lidar_centerpoint { cudaError_t generateSweepPoints_launch( const float * input_points, size_t points_size, int input_point_step, float time_lag, @@ -41,6 +41,6 @@ cudaError_t generateFeatures_launch( const float voxel_size_y, const float voxel_size_z, const float range_min_x, const float range_min_y, const float range_min_z, float * features, cudaStream_t stream); -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp similarity index 77% rename from perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp index 3ade5e677cdbe..66384d192ae43 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/preprocess/voxel_generator.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ -#define LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ -#include "lidar_centerpoint/centerpoint_config.hpp" -#include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_centerpoint/centerpoint_config.hpp" +#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class VoxelGeneratorTemplate { @@ -54,6 +54,6 @@ class VoxelGenerator : public VoxelGeneratorTemplate std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) override; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp similarity index 80% rename from perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp index 484fbcfd36657..de901b06e83c3 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__ROS_UTILS_HPP_ -#define LIDAR_CENTERPOINT__ROS_UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__ROS_UTILS_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__ROS_UTILS_HPP_ // ros packages cannot be included from cuda. -#include "lidar_centerpoint/utils.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { void box3DToDetectedObject( @@ -39,6 +39,6 @@ std::array convertTwistCovarianceMatrix(const Box3D & box3d); bool isCarLikeVehicleLabel(const uint8_t label); -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__ROS_UTILS_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__ROS_UTILS_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/utils.hpp similarity index 83% rename from perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp rename to perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/utils.hpp index f3dd30f754989..9dfb451125b6a 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/utils.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/utils.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_CENTERPOINT__UTILS_HPP_ -#define LIDAR_CENTERPOINT__UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_CENTERPOINT__UTILS_HPP_ +#define AUTOWARE__LIDAR_CENTERPOINT__UTILS_HPP_ #include -namespace centerpoint +namespace autoware::lidar_centerpoint { struct Box3D { @@ -49,6 +49,6 @@ struct Box3D // cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b); -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint -#endif // LIDAR_CENTERPOINT__UTILS_HPP_ +#endif // AUTOWARE__LIDAR_CENTERPOINT__UTILS_HPP_ diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml similarity index 85% rename from perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml rename to perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 950dfa7f5b290..639502d52806a 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -5,7 +5,7 @@ - + @@ -15,7 +15,7 @@ - + @@ -28,7 +28,7 @@ - + diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp similarity index 96% rename from perception/lidar_centerpoint/lib/centerpoint_trt.cpp rename to perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 5040028d1bd94..a1706d1f5601d 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/centerpoint_trt.hpp" +#include "autoware/lidar_centerpoint/centerpoint_trt.hpp" -#include "lidar_centerpoint/centerpoint_config.hpp" -#include "lidar_centerpoint/network/scatter_kernel.hpp" -#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_centerpoint/centerpoint_config.hpp" +#include "autoware/lidar_centerpoint/network/scatter_kernel.hpp" +#include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { CenterPointTRT::CenterPointTRT( const NetworkParam & encoder_param, const NetworkParam & head_param, @@ -200,4 +200,4 @@ void CenterPointTRT::postProcess(std::vector & det_boxes3d) } } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/autoware_lidar_centerpoint/lib/detection_class_remapper.cpp similarity index 94% rename from perception/lidar_centerpoint/lib/detection_class_remapper.cpp rename to perception/autoware_lidar_centerpoint/lib/detection_class_remapper.cpp index 9805fc7a661d1..80c842f8746a4 100644 --- a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp +++ b/perception/autoware_lidar_centerpoint/lib/detection_class_remapper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/detection_class_remapper.hpp" +#include "autoware/lidar_centerpoint/detection_class_remapper.hpp" -namespace centerpoint +namespace autoware::lidar_centerpoint { void DetectionClassRemapper::setParameters( @@ -68,4 +68,4 @@ void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedO } } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp similarity index 95% rename from perception/lidar_centerpoint/lib/network/network_trt.cpp rename to perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp index ad1a7ae90630c..e7484b21d2fd2 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/network/network_trt.hpp" +#include "autoware/lidar_centerpoint/network/network_trt.hpp" -namespace centerpoint +namespace autoware::lidar_centerpoint { bool VoxelEncoderTRT::setProfile( nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, @@ -79,4 +79,4 @@ bool HeadTRT::setProfile( return true; } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu b/perception/autoware_lidar_centerpoint/lib/network/scatter_kernel.cu similarity index 92% rename from perception/lidar_centerpoint/lib/network/scatter_kernel.cu rename to perception/autoware_lidar_centerpoint/lib/network/scatter_kernel.cu index f139e230426ca..7b226a8e7bac0 100644 --- a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/network/scatter_kernel.cu @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/network/scatter_kernel.hpp" -#include "lidar_centerpoint/utils.hpp" +#include "autoware/lidar_centerpoint/network/scatter_kernel.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" namespace { const std::size_t THREADS_PER_BLOCK = 32; } // namespace -namespace centerpoint +namespace autoware::lidar_centerpoint { __global__ void scatterFeatures_kernel( const float * pillar_features, const int * coords, const unsigned int * num_pillars, @@ -64,4 +64,4 @@ cudaError_t scatterFeatures_launch( return cudaGetLastError(); } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp similarity index 97% rename from perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp rename to perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index bdfde42a12a02..9e926aa29846d 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/network/tensorrt_wrapper.hpp" +#include "autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp" #include "NvOnnxParser.h" @@ -20,7 +20,7 @@ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) { @@ -164,4 +164,4 @@ bool TensorRTWrapper::loadEngine(const std::string & engine_path) return true; } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu b/perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu similarity index 94% rename from perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu rename to perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu index c208eefe135fb..63a7d1625f66a 100644 --- a/perception/lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/circle_nms_kernel.cu @@ -21,9 +21,9 @@ Written by Shaoshuai Shi All Rights Reserved 2019-2020. */ -#include "lidar_centerpoint/cuda_utils.hpp" -#include "lidar_centerpoint/postprocess/circle_nms_kernel.hpp" -#include "lidar_centerpoint/utils.hpp" +#include "autoware/lidar_centerpoint/cuda_utils.hpp" +#include "autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" #include "thrust/host_vector.h" namespace @@ -31,7 +31,7 @@ namespace const std::size_t THREADS_PER_BLOCK_NMS = 16; } // namespace -namespace centerpoint +namespace autoware::lidar_centerpoint { __device__ inline float dist2dPow(const Box3D * a, const Box3D * b) @@ -140,4 +140,4 @@ std::size_t circleNMS( return num_to_keep; } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp similarity index 95% rename from perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp rename to perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 16f4fc89bbcd6..f7dd85b019250 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { void NonMaximumSuppression::setParameters(const NMSParams & params) @@ -101,4 +101,4 @@ std::vector NonMaximumSuppression::apply( return output_objects; } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu similarity index 97% rename from perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu rename to perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 12835bab038a6..e1af8f36eb082 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/postprocess/circle_nms_kernel.hpp" -#include "lidar_centerpoint/postprocess/postprocess_kernel.hpp" +#include "autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp" +#include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp" #include "thrust/count.h" #include "thrust/device_vector.h" #include "thrust/sort.h" @@ -23,7 +23,7 @@ namespace const std::size_t THREADS_PER_BLOCK = 32; } // namespace -namespace centerpoint +namespace autoware::lidar_centerpoint { struct is_score_greater @@ -192,4 +192,4 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( return cudaGetLastError(); } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp b/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp similarity index 95% rename from perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp rename to perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp index 2859d58d8f669..066bc5d995228 100644 --- a/perception/lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/pointcloud_densification.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp" #include "pcl_conversions/pcl_conversions.h" #include "pcl_ros/transforms.hpp" @@ -53,7 +53,7 @@ Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) } // namespace -namespace centerpoint +namespace autoware::lidar_centerpoint { PointCloudDensification::PointCloudDensification(const DensificationParam & param) : param_(param) { @@ -111,4 +111,4 @@ void PointCloudDensification::dequeue() } } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu similarity index 98% rename from perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu rename to perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 4e78176739c26..84d519cddf04c 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -28,9 +28,9 @@ * limitations under the License. */ -#include "lidar_centerpoint/cuda_utils.hpp" -#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" -#include "lidar_centerpoint/utils.hpp" +#include "autoware/lidar_centerpoint/cuda_utils.hpp" +#include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" #include @@ -41,7 +41,7 @@ const std::size_t WARPS_PER_BLOCK = 4; const std::size_t ENCODER_IN_FEATURE_SIZE = 9; // the same as encoder_in_feature_size_ in config } // namespace -namespace centerpoint +namespace autoware::lidar_centerpoint { __global__ void generateSweepPoints_kernel( @@ -305,4 +305,4 @@ cudaError_t generateFeatures_launch( return cudaGetLastError(); } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp similarity index 91% rename from perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp rename to perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp index 8e91c3b0b36cc..86dc03719ca8e 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/preprocess/voxel_generator.hpp" +#include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp" -#include "lidar_centerpoint/centerpoint_trt.hpp" -#include "lidar_centerpoint/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_centerpoint/centerpoint_trt.hpp" +#include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include -namespace centerpoint +namespace autoware::lidar_centerpoint { VoxelGeneratorTemplate::VoxelGeneratorTemplate( const DensificationParam & param, const CenterPointConfig & config) @@ -81,4 +81,4 @@ std::size_t VoxelGenerator::generateSweepPoints(float * points_d, cudaStream_t s return point_counter; } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp similarity index 97% rename from perception/lidar_centerpoint/lib/ros_utils.cpp rename to perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index 039d340cc2dc4..d7fc0aef204be 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/ros_utils.hpp" +#include "autoware/lidar_centerpoint/ros_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/constants.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -namespace centerpoint +namespace autoware::lidar_centerpoint { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -120,4 +120,4 @@ std::array convertTwistCovarianceMatrix(const Box3D & box3d) return twist_covariance; } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/lib/utils.cpp b/perception/autoware_lidar_centerpoint/lib/utils.cpp similarity index 87% rename from perception/lidar_centerpoint/lib/utils.cpp rename to perception/autoware_lidar_centerpoint/lib/utils.cpp index b6e0a54ab6de9..a9e7a68f6adaa 100644 --- a/perception/lidar_centerpoint/lib/utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/utils.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" #include -namespace centerpoint +namespace autoware::lidar_centerpoint { // cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) @@ -31,4 +31,4 @@ std::size_t divup(const std::size_t a, const std::size_t b) return (a + b - 1) / b; } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint diff --git a/perception/lidar_centerpoint/package.xml b/perception/autoware_lidar_centerpoint/package.xml similarity index 90% rename from perception/lidar_centerpoint/package.xml rename to perception/autoware_lidar_centerpoint/package.xml index 4c6fa471c297a..07f67f7e3c11e 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/autoware_lidar_centerpoint/package.xml @@ -1,9 +1,9 @@ - lidar_centerpoint + autoware_lidar_centerpoint 1.0.0 - The lidar_centerpoint package + The autoware_lidar_centerpoint package Kenzo Lobos-Tsunekawa Koji Minoda Apache License 2.0 diff --git a/perception/lidar_centerpoint/schema/centerpoint.schemal.json b/perception/autoware_lidar_centerpoint/schema/centerpoint.schemal.json similarity index 100% rename from perception/lidar_centerpoint/schema/centerpoint.schemal.json rename to perception/autoware_lidar_centerpoint/schema/centerpoint.schemal.json diff --git a/perception/lidar_centerpoint/schema/centerpoint_ml_package.schema.json b/perception/autoware_lidar_centerpoint/schema/centerpoint_ml_package.schema.json similarity index 100% rename from perception/lidar_centerpoint/schema/centerpoint_ml_package.schema.json rename to perception/autoware_lidar_centerpoint/schema/centerpoint_ml_package.schema.json diff --git a/perception/lidar_centerpoint/schema/centerpoint_tiny_ml_package.schema.json b/perception/autoware_lidar_centerpoint/schema/centerpoint_tiny_ml_package.schema.json similarity index 100% rename from perception/lidar_centerpoint/schema/centerpoint_tiny_ml_package.schema.json rename to perception/autoware_lidar_centerpoint/schema/centerpoint_tiny_ml_package.schema.json diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp similarity index 94% rename from perception/lidar_centerpoint/src/node.cpp rename to perception/autoware_lidar_centerpoint/src/node.cpp index 53554d0a3becf..5b92e5c811a23 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/node.hpp" +#include "autoware/lidar_centerpoint/node.hpp" #include "pcl_ros/transforms.hpp" @@ -29,12 +29,12 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "lidar_centerpoint/centerpoint_config.hpp" -#include "lidar_centerpoint/preprocess/pointcloud_densification.hpp" -#include "lidar_centerpoint/ros_utils.hpp" -#include "lidar_centerpoint/utils.hpp" +#include "autoware/lidar_centerpoint/centerpoint_config.hpp" +#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_centerpoint/ros_utils.hpp" +#include "autoware/lidar_centerpoint/utils.hpp" -namespace centerpoint +namespace autoware::lidar_centerpoint { LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_options) : Node("lidar_center_point", node_options), tf_buffer_(this->get_clock()) @@ -191,7 +191,7 @@ void LidarCenterPointNode::pointCloudCallback( } } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint #include -RCLCPP_COMPONENTS_REGISTER_NODE(centerpoint::LidarCenterPointNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lidar_centerpoint::LidarCenterPointNode) diff --git a/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp b/perception/autoware_lidar_centerpoint/test/test_detection_class_remapper.cpp similarity index 96% rename from perception/lidar_centerpoint/test/test_detection_class_remapper.cpp rename to perception/autoware_lidar_centerpoint/test/test_detection_class_remapper.cpp index 565b258535462..48642bf7227dc 100644 --- a/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_detection_class_remapper.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include TEST(DetectionClassRemapperTest, MapClasses) { - centerpoint::DetectionClassRemapper remapper; + autoware::lidar_centerpoint::DetectionClassRemapper remapper; // Set up the parameters for the remapper // Labels: CAR, TRUCK, TRAILER diff --git a/perception/lidar_centerpoint/test/test_nms.cpp b/perception/autoware_lidar_centerpoint/test/test_nms.cpp similarity index 92% rename from perception/lidar_centerpoint/test/test_nms.cpp rename to perception/autoware_lidar_centerpoint/test/test_nms.cpp index 601871ff57ed7..4a59d4dba98bf 100644 --- a/perception/lidar_centerpoint/test/test_nms.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_nms.cpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include TEST(NonMaximumSuppressionTest, Apply) { - centerpoint::NonMaximumSuppression nms; - centerpoint::NMSParams params; + autoware::lidar_centerpoint::NonMaximumSuppression nms; + autoware::lidar_centerpoint::NMSParams params; params.search_distance_2d_ = 1.0; params.iou_threshold_ = 0.2; - params.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; + params.nms_type_ = autoware::lidar_centerpoint::NMS_TYPE::IoU_BEV; params.target_class_names_ = {"CAR"}; nms.setParameters(params); - std::vector input_objects(4); + std::vector input_objects(4); // Object 1 autoware_perception_msgs::msg::ObjectClassification obj1_classification; @@ -88,7 +88,8 @@ TEST(NonMaximumSuppressionTest, Apply) input_objects[3].shape.dimensions.x = 0.5; input_objects[3].shape.dimensions.y = 0.5; - std::vector output_objects = nms.apply(input_objects); + std::vector output_objects = + nms.apply(input_objects); // Assert the expected number of output objects EXPECT_EQ(output_objects.size(), 3); diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp b/perception/autoware_lidar_centerpoint/test/test_postprocess_kernel.cpp similarity index 97% rename from perception/lidar_centerpoint/test/test_postprocess_kernel.cpp rename to perception/autoware_lidar_centerpoint/test/test_postprocess_kernel.cpp index e75aff416b8aa..83575fa78156f 100644 --- a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_postprocess_kernel.cpp @@ -20,7 +20,7 @@ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { void PostprocessKernelTest::SetUp() @@ -29,6 +29,7 @@ void PostprocessKernelTest::SetUp() constexpr std::size_t class_size{5}; constexpr std::size_t point_feature_size{4}; + const std::size_t cloud_capacity{2000000}; constexpr std::size_t max_voxel_size{100000000}; const std::vector point_cloud_range{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; const std::vector voxel_size{0.32, 0.32, 10.0}; @@ -39,8 +40,8 @@ void PostprocessKernelTest::SetUp() const std::vector yaw_norm_thresholds{0.5, 0.5, 0.5}; constexpr bool has_variance{false}; - config_ptr_ = std::make_unique( - class_size, point_feature_size, max_voxel_size, point_cloud_range, voxel_size, + config_ptr_ = std::make_unique( + class_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); @@ -358,7 +359,7 @@ TEST_F(PostprocessKernelTest, CircleNMSTest) ASSERT_EQ(1, det_boxes3d.size()); } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint int main(int argc, char ** argv) { diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp b/perception/autoware_lidar_centerpoint/test/test_postprocess_kernel.hpp similarity index 81% rename from perception/lidar_centerpoint/test/test_postprocess_kernel.hpp rename to perception/autoware_lidar_centerpoint/test/test_postprocess_kernel.hpp index 8f35b98119afe..40596a12e83cc 100644 --- a/perception/lidar_centerpoint/test/test_postprocess_kernel.hpp +++ b/perception/autoware_lidar_centerpoint/test/test_postprocess_kernel.hpp @@ -15,14 +15,14 @@ #ifndef TEST_POSTPROCESS_KERNEL_HPP_ #define TEST_POSTPROCESS_KERNEL_HPP_ -#include -#include +#include +#include #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class PostprocessKernelTest : public testing::Test @@ -34,7 +34,7 @@ class PostprocessKernelTest : public testing::Test cudaStream_t stream_{}; std::unique_ptr postprocess_cuda_ptr_{}; - std::unique_ptr config_ptr_{}; + std::unique_ptr config_ptr_{}; cuda::unique_ptr head_out_heatmap_d_{}; cuda::unique_ptr head_out_offset_d_{}; @@ -44,6 +44,6 @@ class PostprocessKernelTest : public testing::Test cuda::unique_ptr head_out_vel_d_{}; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint #endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp b/perception/autoware_lidar_centerpoint/test/test_preprocess_kernel.cpp similarity index 97% rename from perception/lidar_centerpoint/test/test_preprocess_kernel.cpp rename to perception/autoware_lidar_centerpoint/test/test_preprocess_kernel.cpp index db75328f9d5c5..b311ae6ddf85c 100644 --- a/perception/lidar_centerpoint/test/test_preprocess_kernel.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_preprocess_kernel.cpp @@ -14,7 +14,7 @@ #include "test_preprocess_kernel.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { void PreprocessKernelTest::SetUp() @@ -101,7 +101,7 @@ TEST_F(PreprocessKernelTest, EmptyVoxelTest) points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), cudaMemcpyHostToDevice)); - cudaError_t code = centerpoint::generateVoxels_random_launch( + cudaError_t code = autoware::lidar_centerpoint::generateVoxels_random_launch( points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); @@ -127,7 +127,7 @@ TEST_F(PreprocessKernelTest, BasicTest) points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), cudaMemcpyHostToDevice)); - cudaError_t code = centerpoint::generateVoxels_random_launch( + cudaError_t code = autoware::lidar_centerpoint::generateVoxels_random_launch( points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); @@ -242,7 +242,7 @@ TEST_F(PreprocessKernelTest, OutOfRangeTest) points_d_.get(), points.data(), points_num * point_feature_size_ * sizeof(float), cudaMemcpyHostToDevice)); - cudaError_t code = centerpoint::generateVoxels_random_launch( + cudaError_t code = autoware::lidar_centerpoint::generateVoxels_random_launch( points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); @@ -275,7 +275,7 @@ TEST_F(PreprocessKernelTest, VoxelOverflowTest) // Note: due to atomic operations in the kernel, generateVoxels does not handle overflows in the // counter, and instead is done in the following kernel - cudaError_t code = centerpoint::generateVoxels_random_launch( + cudaError_t code = autoware::lidar_centerpoint::generateVoxels_random_launch( points_d_.get(), points_num, range_min_x_, range_max_x_, range_min_y_, range_max_y_, range_min_z_, range_max_z_, voxel_size_x_, voxel_size_y_, voxel_size_z_, grid_size_y_, grid_size_x_, mask_d_.get(), voxels_buffer_d_.get(), stream_); @@ -394,7 +394,7 @@ TEST_F(PreprocessKernelTest, VoxelOverflowTest) } } -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint int main(int argc, char ** argv) { diff --git a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp b/perception/autoware_lidar_centerpoint/test/test_preprocess_kernel.hpp similarity index 94% rename from perception/lidar_centerpoint/test/test_preprocess_kernel.hpp rename to perception/autoware_lidar_centerpoint/test/test_preprocess_kernel.hpp index 1a3e850026886..ebf9dd36a360e 100644 --- a/perception/lidar_centerpoint/test/test_preprocess_kernel.hpp +++ b/perception/autoware_lidar_centerpoint/test/test_preprocess_kernel.hpp @@ -15,11 +15,11 @@ #ifndef TEST_PREPROCESS_KERNEL_HPP_ #define TEST_PREPROCESS_KERNEL_HPP_ -#include +#include #include -namespace centerpoint +namespace autoware::lidar_centerpoint { class PreprocessKernelTest : public testing::Test @@ -74,6 +74,6 @@ class PreprocessKernelTest : public testing::Test cuda::unique_ptr num_voxels_d_{}; }; -} // namespace centerpoint +} // namespace autoware::lidar_centerpoint #endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_centerpoint/test/test_ros_utils.cpp b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp similarity index 75% rename from perception/lidar_centerpoint/test/test_ros_utils.cpp rename to perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp index 6044a3b6e77c9..555d820ec3644 100644 --- a/perception/lidar_centerpoint/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_centerpoint/ros_utils.hpp" +#include "autoware/lidar_centerpoint/ros_utils.hpp" #include @@ -23,7 +23,7 @@ TEST(TestSuite, box3DToDetectedObject) // Test case 1: Test with valid label, has_twist=true, has_variance=true { - centerpoint::Box3D box3d; + autoware::lidar_centerpoint::Box3D box3d; box3d.score = 0.8f; box3d.label = 0; // CAR box3d.x = 1.0; @@ -43,7 +43,7 @@ TEST(TestSuite, box3DToDetectedObject) box3d.vel_y_variance = 0.6; autoware_perception_msgs::msg::DetectedObject obj; - centerpoint::box3DToDetectedObject(box3d, class_names, true, true, obj); + autoware::lidar_centerpoint::box3DToDetectedObject(box3d, class_names, true, true, obj); EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); EXPECT_EQ( @@ -61,12 +61,12 @@ TEST(TestSuite, box3DToDetectedObject) // Test case 2: Test with invalid label, has_twist=false, has_variance=false { - centerpoint::Box3D box3d; + autoware::lidar_centerpoint::Box3D box3d; box3d.score = 0.5f; box3d.label = 10; // Invalid autoware_perception_msgs::msg::DetectedObject obj; - centerpoint::box3DToDetectedObject(box3d, class_names, false, false, obj); + autoware::lidar_centerpoint::box3DToDetectedObject(box3d, class_names, false, false, obj); EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); EXPECT_EQ( @@ -80,38 +80,41 @@ TEST(TestSuite, box3DToDetectedObject) TEST(TestSuite, getSemanticType) { EXPECT_EQ( - centerpoint::getSemanticType("CAR"), autoware_perception_msgs::msg::ObjectClassification::CAR); + autoware::lidar_centerpoint::getSemanticType("CAR"), + autoware_perception_msgs::msg::ObjectClassification::CAR); EXPECT_EQ( - centerpoint::getSemanticType("TRUCK"), + autoware::lidar_centerpoint::getSemanticType("TRUCK"), autoware_perception_msgs::msg::ObjectClassification::TRUCK); EXPECT_EQ( - centerpoint::getSemanticType("BUS"), autoware_perception_msgs::msg::ObjectClassification::BUS); + autoware::lidar_centerpoint::getSemanticType("BUS"), + autoware_perception_msgs::msg::ObjectClassification::BUS); EXPECT_EQ( - centerpoint::getSemanticType("TRAILER"), + autoware::lidar_centerpoint::getSemanticType("TRAILER"), autoware_perception_msgs::msg::ObjectClassification::TRAILER); EXPECT_EQ( - centerpoint::getSemanticType("BICYCLE"), + autoware::lidar_centerpoint::getSemanticType("BICYCLE"), autoware_perception_msgs::msg::ObjectClassification::BICYCLE); EXPECT_EQ( - centerpoint::getSemanticType("MOTORBIKE"), + autoware::lidar_centerpoint::getSemanticType("MOTORBIKE"), autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); EXPECT_EQ( - centerpoint::getSemanticType("PEDESTRIAN"), + autoware::lidar_centerpoint::getSemanticType("PEDESTRIAN"), autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN); EXPECT_EQ( - centerpoint::getSemanticType("UNKNOWN"), + autoware::lidar_centerpoint::getSemanticType("UNKNOWN"), autoware_perception_msgs::msg::ObjectClassification::UNKNOWN); } TEST(TestSuite, convertPoseCovarianceMatrix) { - centerpoint::Box3D box3d; + autoware::lidar_centerpoint::Box3D box3d; box3d.x_variance = 0.1; box3d.y_variance = 0.2; box3d.z_variance = 0.3; box3d.yaw_variance = 0.4; - std::array pose_covariance = centerpoint::convertPoseCovarianceMatrix(box3d); + std::array pose_covariance = + autoware::lidar_centerpoint::convertPoseCovarianceMatrix(box3d); EXPECT_FLOAT_EQ(pose_covariance[0], 0.1); EXPECT_FLOAT_EQ(pose_covariance[7], 0.2); @@ -121,11 +124,12 @@ TEST(TestSuite, convertPoseCovarianceMatrix) TEST(TestSuite, convertTwistCovarianceMatrix) { - centerpoint::Box3D box3d; + autoware::lidar_centerpoint::Box3D box3d; box3d.vel_x_variance = 0.1; box3d.vel_y_variance = 0.2; - std::array twist_covariance = centerpoint::convertTwistCovarianceMatrix(box3d); + std::array twist_covariance = + autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d); EXPECT_FLOAT_EQ(twist_covariance[0], 0.1); EXPECT_FLOAT_EQ(twist_covariance[7], 0.2); diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.cpp b/perception/autoware_lidar_centerpoint/test/test_voxel_generator.cpp similarity index 85% rename from perception/lidar_centerpoint/test/test_voxel_generator.cpp rename to perception/autoware_lidar_centerpoint/test/test_voxel_generator.cpp index 6bded105536dc..a5b5337e3b24e 100644 --- a/perception/lidar_centerpoint/test/test_voxel_generator.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_voxel_generator.cpp @@ -31,6 +31,7 @@ void VoxelGeneratorTest::SetUp() class_size_ = 5; point_feature_size_ = 4; + cloud_capacity_ = 2000000; max_voxel_size_ = 100000000; point_cloud_range_ = std::vector{-76.8, -76.8, -4.0, 76.8, 76.8, 6.0}; voxel_size_ = std::vector{0.32, 0.32, 10.0}; @@ -107,14 +108,14 @@ TEST_F(VoxelGeneratorTest, SingleFrame) { const unsigned int num_past_frames = 0; // only current frame - centerpoint::DensificationParam param(world_frame_, num_past_frames); + autoware::lidar_centerpoint::DensificationParam param(world_frame_, num_past_frames); - centerpoint::CenterPointConfig config( - class_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_); + autoware::lidar_centerpoint::CenterPointConfig config( + class_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_); - centerpoint::VoxelGenerator voxel_generator(param, config); + autoware::lidar_centerpoint::VoxelGenerator voxel_generator(param, config); std::vector points; points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); @@ -154,14 +155,14 @@ TEST_F(VoxelGeneratorTest, TwoFramesNoTf) { const unsigned int num_past_frames = 1; - centerpoint::DensificationParam param(world_frame_, num_past_frames); + autoware::lidar_centerpoint::DensificationParam param(world_frame_, num_past_frames); - centerpoint::CenterPointConfig config( - class_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_); + autoware::lidar_centerpoint::CenterPointConfig config( + class_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_); - centerpoint::VoxelGenerator voxel_generator(param, config); + autoware::lidar_centerpoint::VoxelGenerator voxel_generator(param, config); std::vector points; points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); @@ -188,14 +189,14 @@ TEST_F(VoxelGeneratorTest, TwoFrames) { const unsigned int num_past_frames = 1; - centerpoint::DensificationParam param(world_frame_, num_past_frames); + autoware::lidar_centerpoint::DensificationParam param(world_frame_, num_past_frames); - centerpoint::CenterPointConfig config( - class_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_); + autoware::lidar_centerpoint::CenterPointConfig config( + class_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_); - centerpoint::VoxelGenerator voxel_generator(param, config); + autoware::lidar_centerpoint::VoxelGenerator voxel_generator(param, config); std::vector points; points.resize(capacity_ * config.point_feature_size_); std::fill(points.begin(), points.end(), std::nan("")); diff --git a/perception/lidar_centerpoint/test/test_voxel_generator.hpp b/perception/autoware_lidar_centerpoint/test/test_voxel_generator.hpp similarity index 86% rename from perception/lidar_centerpoint/test/test_voxel_generator.hpp rename to perception/autoware_lidar_centerpoint/test/test_voxel_generator.hpp index 48355b8a331ba..5e6ab3c712ba5 100644 --- a/perception/lidar_centerpoint/test/test_voxel_generator.hpp +++ b/perception/autoware_lidar_centerpoint/test/test_voxel_generator.hpp @@ -15,7 +15,7 @@ #ifndef TEST_VOXEL_GENERATOR_HPP_ #define TEST_VOXEL_GENERATOR_HPP_ -#include +#include #include #include @@ -38,8 +38,8 @@ class VoxelGeneratorTest : public testing::Test std::unique_ptr cloud1_{}, cloud2_{}; geometry_msgs::msg::TransformStamped transform1_{}, transform2_{}; - std::unique_ptr densification_param_ptr_{}; - std::unique_ptr config_ptr_{}; + std::unique_ptr densification_param_ptr_{}; + std::unique_ptr config_ptr_{}; std::unique_ptr tf2_buffer_{}; @@ -51,6 +51,7 @@ class VoxelGeneratorTest : public testing::Test std::size_t class_size_{}; float point_feature_size_{}; + std::size_t cloud_capacity_{}; std::size_t max_voxel_size_{}; std::vector point_cloud_range_{}; diff --git a/perception/lidar_transfusion/CMakeLists.txt b/perception/autoware_lidar_transfusion/CMakeLists.txt similarity index 88% rename from perception/lidar_transfusion/CMakeLists.txt rename to perception/autoware_lidar_transfusion/CMakeLists.txt index 15c89beb15d3e..c3a56f883fbe5 100644 --- a/perception/lidar_transfusion/CMakeLists.txt +++ b/perception/autoware_lidar_transfusion/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(lidar_transfusion) +project(autoware_lidar_transfusion) find_package(autoware_cmake REQUIRED) autoware_package() @@ -82,7 +82,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) set(CUDA_NVCC_FLAGS "-g -G") endif() - ament_auto_add_library(transfusion_lib SHARED + ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/detection_class_remapper.cpp lib/network/network_trt.cpp lib/postprocess/non_maximum_suppression.cpp @@ -92,23 +92,23 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/transfusion_trt.cpp ) - cuda_add_library(transfusion_cuda_lib SHARED + cuda_add_library(${PROJECT_NAME}_cuda_lib SHARED lib/postprocess/circle_nms_kernel.cu lib/postprocess/postprocess_kernel.cu lib/preprocess/preprocess_kernel.cu ) - target_link_libraries(transfusion_lib + target_link_libraries(${PROJECT_NAME}_lib ${NVINFER} ${NVONNXPARSER} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} - transfusion_cuda_lib + ${PROJECT_NAME}_cuda_lib ) - target_include_directories(transfusion_lib + target_include_directories(${PROJECT_NAME}_lib PUBLIC $ $ @@ -116,26 +116,26 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6. # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe - target_include_directories(transfusion_lib + target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC ${CUDA_INCLUDE_DIRS} ) - ament_auto_add_library(lidar_transfusion_component SHARED + ament_auto_add_library(${PROJECT_NAME}_component SHARED src/lidar_transfusion_node.cpp ) - target_link_libraries(lidar_transfusion_component - transfusion_lib + target_link_libraries(${PROJECT_NAME}_component + ${PROJECT_NAME}_lib ) - rclcpp_components_register_node(lidar_transfusion_component - PLUGIN "lidar_transfusion::LidarTransfusionNode" - EXECUTABLE lidar_transfusion_node + rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "autoware::lidar_transfusion::LidarTransfusionNode" + EXECUTABLE ${PROJECT_NAME}_node ) install( - TARGETS transfusion_cuda_lib + TARGETS ${PROJECT_NAME}_cuda_lib DESTINATION lib ) @@ -168,7 +168,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # ${test_preprocess_kernel_SOURCE_DIR} # ) # target_link_libraries(test_preprocess_kernel - # transfusion_cuda_lib + # ${PROJECT_NAME}_cuda_lib # gtest # gtest_main # ) @@ -185,7 +185,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # ${test_postprocess_kernel_SOURCE_DIR} # ) # target_link_libraries(test_postprocess_kernel - # transfusion_cuda_lib + # ${PROJECT_NAME}_cuda_lib # gtest # gtest_main # ) diff --git a/perception/lidar_transfusion/README.md b/perception/autoware_lidar_transfusion/README.md similarity index 84% rename from perception/lidar_transfusion/README.md rename to perception/autoware_lidar_transfusion/README.md index 6745cc1f7d219..4a55babbcba9f 100644 --- a/perception/lidar_transfusion/README.md +++ b/perception/autoware_lidar_transfusion/README.md @@ -1,8 +1,8 @@ -# lidar_transfusion +# autoware_lidar_transfusion ## Purpose -The `lidar_transfusion` package is used for 3D object detection based on lidar data (x, y, z, intensity). +The `autoware_lidar_transfusion` package is used for 3D object detection based on lidar data (x, y, z, intensity). ## Inner-workings / Algorithms @@ -34,27 +34,27 @@ We trained the models using . ### TransFusion -{{ json_to_markdown("perception/lidar_transfusion/schema/transfusion.schema.json") }} +{{ json_to_markdown("perception/autoware_lidar_transfusion/schema/transfusion.schema.json") }} ### Detection class remapper -{{ json_to_markdown("perception/lidar_transfusion/schema/detection_class_remapper.schema.json") }} +{{ json_to_markdown("perception/autoware_lidar_transfusion/schema/detection_class_remapper.schema.json") }} ### The `build_only` option -The `lidar_transfusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. +The `autoware_lidar_transfusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: ```bash -ros2 launch lidar_transfusion lidar_transfusion.launch.xml build_only:=true +ros2 launch autoware_lidar_transfusion lidar_transfusion.launch.xml build_only:=true ``` ### The `log_level` option -The default logging severity level for `lidar_transfusion` is `info`. For debugging purposes, the developer may decrease severity level using `log_level` parameter: +The default logging severity level for `autoware_lidar_transfusion` is `info`. For debugging purposes, the developer may decrease severity level using `log_level` parameter: ```bash -ros2 launch lidar_transfusion lidar_transfusion.launch.xml log_level:=debug +ros2 launch autoware_lidar_transfusion lidar_transfusion.launch.xml log_level:=debug ``` ## Assumptions / Known limits diff --git a/perception/lidar_transfusion/config/detection_class_remapper.param.yaml b/perception/autoware_lidar_transfusion/config/detection_class_remapper.param.yaml similarity index 100% rename from perception/lidar_transfusion/config/detection_class_remapper.param.yaml rename to perception/autoware_lidar_transfusion/config/detection_class_remapper.param.yaml diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/autoware_lidar_transfusion/config/transfusion.param.yaml similarity index 100% rename from perception/lidar_transfusion/config/transfusion.param.yaml rename to perception/autoware_lidar_transfusion/config/transfusion.param.yaml diff --git a/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/cuda_utils.hpp similarity index 96% rename from perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/cuda_utils.hpp index 5c25a936d5392..17a7c7ee9c165 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/cuda_utils.hpp @@ -39,8 +39,8 @@ * https://creativecommons.org/publicdomain/zero/1.0/deed.en */ -#ifndef LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ -#define LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ #include @@ -123,4 +123,4 @@ void clear_async(T * ptr, size_t num_elem, cudaStream_t stream) } // namespace cuda -#endif // LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/detection_class_remapper.hpp similarity index 82% rename from perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/detection_class_remapper.hpp index ace7dc5ff3405..6095472affe92 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/detection_class_remapper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ -#define LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ #include @@ -24,7 +24,7 @@ #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class DetectionClassRemapper @@ -42,6 +42,6 @@ class DetectionClassRemapper int num_labels_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp similarity index 78% rename from perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp index a82013fac6ce7..e19d2d49af998 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/lidar_transfusion_node.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ -#define LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ -#include "lidar_transfusion/detection_class_remapper.hpp" -#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" -#include "lidar_transfusion/transfusion_trt.hpp" -#include "lidar_transfusion/visibility_control.hpp" +#include "autoware/lidar_transfusion/detection_class_remapper.hpp" +#include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/transfusion_trt.hpp" +#include "autoware/lidar_transfusion/visibility_control.hpp" #include #include @@ -36,7 +36,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node @@ -68,6 +68,6 @@ class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node std::unique_ptr debug_publisher_ptr_{nullptr}; std::unique_ptr published_time_pub_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp similarity index 86% rename from perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp index 3f8f32e346b0a..d237361a436e2 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ -#define LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include @@ -29,7 +29,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { struct ProfileDimension @@ -82,6 +82,6 @@ class NetworkTRT std::array in_profile_dims_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp similarity index 72% rename from perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp index e231482652b98..653bc281c7086 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#define LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { // Non-maximum suppression (NMS) uses the distance on the xy plane instead of // intersection over union (IoU) to suppress overlapped objects. @@ -27,6 +27,6 @@ std::size_t circleNMS( thrust::device_vector & boxes3d, const float distance_threshold, thrust::device_vector & keep_mask, cudaStream_t stream); -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp similarity index 83% rename from perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp index daf2cc7c1fac1..9f07d2448fcae 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ -#define LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ -#include "lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { using autoware_perception_msgs::msg::DetectedObject; @@ -75,6 +75,6 @@ class NonMaximumSuppression std::vector target_class_mask_{}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp similarity index 74% rename from perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp index 01435424aa248..ed677bc84b867 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ -#define LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include #include @@ -24,7 +24,7 @@ #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PostprocessCuda @@ -45,6 +45,6 @@ class PostprocessCuda thrust::device_vector yaw_norm_thresholds_d_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp similarity index 88% rename from perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp index 6ac0a6544389f..2d03978497f72 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#define LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ -#include "lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" #include #include @@ -29,7 +29,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class DensificationParam @@ -96,6 +96,6 @@ class PointCloudDensification cudaStream_t stream_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp similarity index 83% rename from perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp index 592f09c2d288a..1f5fe3ae3907f 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp @@ -28,16 +28,16 @@ * limitations under the License. */ -#ifndef LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ -#define LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PreprocessCuda @@ -68,6 +68,6 @@ class PreprocessCuda unsigned int mask_size_; unsigned int voxels_size_; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/voxel_generator.hpp similarity index 76% rename from perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/voxel_generator.hpp index f0d253ee28755..19bd8c14787de 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/preprocess/voxel_generator.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ -#define LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" -#include "lidar_transfusion/ros_utils.hpp" -#include "lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" #ifdef ROS_DISTRO_GALACTIC #include @@ -36,7 +36,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix constexpr size_t MAX_CLOUD_STEP_SIZE = sizeof(autoware_point_types::PointXYZIRCAEDT); @@ -67,6 +67,6 @@ class VoxelGenerator bool is_initialized_{false}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/ros_utils.hpp similarity index 93% rename from perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/ros_utils.hpp index cbfc4e87b1610..37b364ecf8a9d 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/ros_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__ROS_UTILS_HPP_ -#define LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__ROS_UTILS_HPP_ -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include @@ -41,7 +41,7 @@ CHECK_OFFSET(offset, structure, field); \ CHECK_TYPE(type, structure, field) -namespace lidar_transfusion +namespace autoware::lidar_transfusion { using sensor_msgs::msg::PointField; @@ -102,6 +102,6 @@ void box3DToDetectedObject( uint8_t getSemanticType(const std::string & class_name); -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__ROS_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp similarity index 95% rename from perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp index 0d0148d2f6c17..363ee17a0d6e0 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ -#define LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class TransfusionConfig @@ -165,6 +165,6 @@ class TransfusionConfig std::size_t max_coors_dim_size_{num_point_values_}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp similarity index 80% rename from perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp index 0a9ea413dc30d..4ba3474fc1d72 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ -#define LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ - -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/network/network_trt.hpp" -#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" -#include "lidar_transfusion/preprocess/voxel_generator.hpp" -#include "lidar_transfusion/utils.hpp" -#include "lidar_transfusion/visibility_control.hpp" +#ifndef AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ + +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/network/network_trt.hpp" +#include "autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/preprocess/voxel_generator.hpp" +#include "autoware/lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/visibility_control.hpp" #include @@ -37,7 +37,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class NetworkParam @@ -109,6 +109,6 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT cuda::unique_ptr dir_cls_output_d_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp similarity index 84% rename from perception/lidar_transfusion/include/lidar_transfusion/utils.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp index cc40e55851738..feea2216c5f78 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__UTILS_HPP_ -#define LIDAR_TRANSFUSION__UTILS_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__UTILS_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__UTILS_HPP_ #include #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { struct Box3D @@ -52,6 +52,6 @@ unsigned int divup(const T1 a, const T2 b) return (a + b - 1) / b; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion -#endif // LIDAR_TRANSFUSION__UTILS_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/visibility_control.hpp similarity index 89% rename from perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp rename to perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/visibility_control.hpp index aeab20906628a..fe8bea1ae1403 100644 --- a/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ -#define LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml b/perception/autoware_lidar_transfusion/launch/lidar_transfusion.launch.xml similarity index 79% rename from perception/lidar_transfusion/launch/lidar_transfusion.launch.xml rename to perception/autoware_lidar_transfusion/launch/lidar_transfusion.launch.xml index 492eb8c4d59b7..c14802dcddc55 100644 --- a/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml +++ b/perception/autoware_lidar_transfusion/launch/lidar_transfusion.launch.xml @@ -4,8 +4,8 @@ - - + + @@ -14,7 +14,7 @@ - + @@ -26,7 +26,7 @@ - + diff --git a/perception/lidar_transfusion/lib/detection_class_remapper.cpp b/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp similarity index 94% rename from perception/lidar_transfusion/lib/detection_class_remapper.cpp rename to perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp index e093637402448..08bdc65e666de 100644 --- a/perception/lidar_transfusion/lib/detection_class_remapper.cpp +++ b/perception/autoware_lidar_transfusion/lib/detection_class_remapper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void DetectionClassRemapper::setParameters( @@ -68,4 +68,4 @@ void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedO } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp similarity index 98% rename from perception/lidar_transfusion/lib/network/network_trt.cpp rename to perception/autoware_lidar_transfusion/lib/network/network_trt.cpp index a36890facceec..3eacf005cff4e 100644 --- a/perception/lidar_transfusion/lib/network/network_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/network/network_trt.hpp" +#include "autoware/lidar_transfusion/network/network_trt.hpp" #include @@ -20,7 +20,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) @@ -329,4 +329,4 @@ nvinfer1::Dims NetworkTRT::validateTensorShape(NetworkIO name, const std::vector return tensor_shape; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu b/perception/autoware_lidar_transfusion/lib/postprocess/circle_nms_kernel.cu similarity index 94% rename from perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu rename to perception/autoware_lidar_transfusion/lib/postprocess/circle_nms_kernel.cu index dcb60831825bb..2328ca53af530 100644 --- a/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/postprocess/circle_nms_kernel.cu @@ -21,9 +21,9 @@ Written by Shaoshuai Shi All Rights Reserved 2019-2020. */ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include @@ -32,7 +32,7 @@ namespace const std::size_t THREADS_PER_BLOCK_NMS = 16; } // namespace -namespace lidar_transfusion +namespace autoware::lidar_transfusion { __device__ inline float dist2dPow(const Box3D * a, const Box3D * b) @@ -141,4 +141,4 @@ std::size_t circleNMS( return num_to_keep; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp similarity index 95% rename from perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp rename to perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index dd12c52970d38..6e563f706e3b2 100644 --- a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void NonMaximumSuppression::setParameters(const NMSParams & params) @@ -101,4 +101,4 @@ std::vector NonMaximumSuppression::apply( return output_objects; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu similarity index 95% rename from perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu rename to perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu index bca23e9961b40..fbdb89d59b3d1 100644 --- a/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" -#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" +#include "autoware/lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp" #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { const size_t THREADS_PER_BLOCK = 256; @@ -142,4 +142,4 @@ cudaError_t PostprocessCuda::generateDetectedBoxes3D_launch( return cudaGetLastError(); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp b/perception/autoware_lidar_transfusion/lib/preprocess/pointcloud_densification.cpp similarity index 95% rename from perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp rename to perception/autoware_lidar_transfusion/lib/preprocess/pointcloud_densification.cpp index c13423f2d24d8..17491ede058b4 100644 --- a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp +++ b/perception/autoware_lidar_transfusion/lib/preprocess/pointcloud_densification.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" #include @@ -55,7 +55,7 @@ Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) } // namespace -namespace lidar_transfusion +namespace autoware::lidar_transfusion { PointCloudDensification::PointCloudDensification( @@ -113,4 +113,4 @@ void PointCloudDensification::dequeue() } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu similarity index 97% rename from perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu rename to perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu index 48bb4eb9332a8..1b18b3dbcc006 100644 --- a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -28,12 +28,12 @@ * limitations under the License. */ -#include "lidar_transfusion/cuda_utils.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { PreprocessCuda::PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) @@ -218,4 +218,4 @@ cudaError_t PreprocessCuda::generateSweepPoints_launch( return err; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp similarity index 95% rename from perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp rename to perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp index cb8bac984aef3..203a556d9057a 100644 --- a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp +++ b/perception/autoware_lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/preprocess/voxel_generator.hpp" +#include "autoware/lidar_transfusion/preprocess/voxel_generator.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { VoxelGenerator::VoxelGenerator( @@ -120,4 +120,4 @@ std::tuple VoxelGenerator::getFiel } throw std::runtime_error("Missing field: " + field_name); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp similarity index 95% rename from perception/lidar_transfusion/lib/ros_utils.cpp rename to perception/autoware_lidar_transfusion/lib/ros_utils.cpp index ef5c45c339b64..dce4028759de5 100644 --- a/perception/lidar_transfusion/lib/ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/lib/ros_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" #include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -80,4 +80,4 @@ uint8_t getSemanticType(const std::string & class_name) } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp similarity index 96% rename from perception/lidar_transfusion/lib/transfusion_trt.cpp rename to perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp index d940b83c12cb4..94760d4d91720 100644 --- a/perception/lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/transfusion_trt.hpp" +#include "autoware/lidar_transfusion/transfusion_trt.hpp" -#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" -#include "lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { TransfusionTRT::TransfusionTRT( @@ -203,4 +203,4 @@ bool TransfusionTRT::postprocess(std::vector & det_boxes3d) return true; } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion diff --git a/perception/lidar_transfusion/package.xml b/perception/autoware_lidar_transfusion/package.xml similarity index 96% rename from perception/lidar_transfusion/package.xml rename to perception/autoware_lidar_transfusion/package.xml index 3b495025b1c34..834afba1221ad 100644 --- a/perception/lidar_transfusion/package.xml +++ b/perception/autoware_lidar_transfusion/package.xml @@ -1,7 +1,7 @@ - lidar_transfusion + autoware_lidar_transfusion 1.0.0 The lidar_transfusion package Amadeusz Szymko diff --git a/perception/lidar_transfusion/schema/detection_class_remapper.schema.json b/perception/autoware_lidar_transfusion/schema/detection_class_remapper.schema.json similarity index 100% rename from perception/lidar_transfusion/schema/detection_class_remapper.schema.json rename to perception/autoware_lidar_transfusion/schema/detection_class_remapper.schema.json diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/autoware_lidar_transfusion/schema/transfusion.schema.json similarity index 100% rename from perception/lidar_transfusion/schema/transfusion.schema.json rename to perception/autoware_lidar_transfusion/schema/transfusion.schema.json diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp similarity index 96% rename from perception/lidar_transfusion/src/lidar_transfusion_node.cpp rename to perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index a07e385208748..fae1cc97404da 100644 --- a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/lidar_transfusion_node.hpp" +#include "autoware/lidar_transfusion/lidar_transfusion_node.hpp" -#include "lidar_transfusion/utils.hpp" +#include "autoware/lidar_transfusion/utils.hpp" -namespace lidar_transfusion +namespace autoware::lidar_transfusion { LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) @@ -175,8 +175,8 @@ void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::Co } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(lidar_transfusion::LidarTransfusionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lidar_transfusion::LidarTransfusionNode) diff --git a/perception/lidar_transfusion/test/test_detection_class_remapper.cpp b/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp similarity index 96% rename from perception/lidar_transfusion/test/test_detection_class_remapper.cpp rename to perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp index 6e7f896e44d2c..a9b5c8e83fbf7 100644 --- a/perception/lidar_transfusion/test/test_detection_class_remapper.cpp +++ b/perception/autoware_lidar_transfusion/test/test_detection_class_remapper.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include TEST(DetectionClassRemapperTest, MapClasses) { - lidar_transfusion::DetectionClassRemapper remapper; + autoware::lidar_transfusion::DetectionClassRemapper remapper; // Set up the parameters for the remapper // Labels: CAR, TRUCK, TRAILER diff --git a/perception/lidar_transfusion/test/test_nms.cpp b/perception/autoware_lidar_transfusion/test/test_nms.cpp similarity index 92% rename from perception/lidar_transfusion/test/test_nms.cpp rename to perception/autoware_lidar_transfusion/test/test_nms.cpp index b6f0ec8c31684..654cb56078c57 100644 --- a/perception/lidar_transfusion/test/test_nms.cpp +++ b/perception/autoware_lidar_transfusion/test/test_nms.cpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp" #include TEST(NonMaximumSuppressionTest, Apply) { - lidar_transfusion::NonMaximumSuppression nms; - lidar_transfusion::NMSParams params; + autoware::lidar_transfusion::NonMaximumSuppression nms; + autoware::lidar_transfusion::NMSParams params; params.search_distance_2d_ = 1.0; params.iou_threshold_ = 0.2; - params.nms_type_ = lidar_transfusion::NMS_TYPE::IoU_BEV; + params.nms_type_ = autoware::lidar_transfusion::NMS_TYPE::IoU_BEV; params.target_class_names_ = {"CAR"}; nms.setParameters(params); - std::vector input_objects(4); + std::vector input_objects(4); // Object 1 autoware_perception_msgs::msg::ObjectClassification obj1_classification; @@ -88,7 +88,8 @@ TEST(NonMaximumSuppressionTest, Apply) input_objects[3].shape.dimensions.x = 0.5; input_objects[3].shape.dimensions.y = 0.5; - std::vector output_objects = nms.apply(input_objects); + std::vector output_objects = + nms.apply(input_objects); // Assert the expected number of output objects EXPECT_EQ(output_objects.size(), 3); diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.cpp b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp similarity index 97% rename from perception/lidar_transfusion/test/test_postprocess_kernel.cpp rename to perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp index 1e01db5a8b3d7..bc30ca04ebfe6 100644 --- a/perception/lidar_transfusion/test/test_postprocess_kernel.cpp +++ b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.cpp @@ -19,22 +19,24 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void PostprocessKernelTest::SetUp() { cudaStreamCreate(&stream_); + auto cloud_capacity = 2000000; auto voxels_num = std::vector{1, 3, 5}; auto point_cloud_range = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; auto voxel_size = std::vector{0.3, 0.3, 8.0}; + auto num_proposals = 500; auto score_threshold = 0.2f; auto circle_nms_dist_threshold = 0.5f; auto yaw_norm_thresholds = std::vector{0.5, 0.5, 0.5}; config_ptr_ = std::make_unique( - voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, - score_threshold); + cloud_capacity, voxels_num, point_cloud_range, voxel_size, num_proposals, + circle_nms_dist_threshold, yaw_norm_thresholds, score_threshold); post_ptr_ = std::make_unique(*config_ptr_, stream_); cls_size_ = config_ptr_->num_proposals_ * config_ptr_->num_classes_; @@ -290,7 +292,7 @@ TEST_F(PostprocessKernelTest, CircleNMSTest) EXPECT_EQ(1, det_boxes3d.size()); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion int main(int argc, char ** argv) { diff --git a/perception/lidar_transfusion/test/test_postprocess_kernel.hpp b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.hpp similarity index 85% rename from perception/lidar_transfusion/test/test_postprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/test/test_postprocess_kernel.hpp index 60fa7882ecc11..104f42ad9e75d 100644 --- a/perception/lidar_transfusion/test/test_postprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/test/test_postprocess_kernel.hpp @@ -15,14 +15,14 @@ #ifndef TEST_POSTPROCESS_KERNEL_HPP_ #define TEST_POSTPROCESS_KERNEL_HPP_ -#include -#include +#include +#include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PostprocessKernelTest : public testing::Test @@ -43,6 +43,6 @@ class PostprocessKernelTest : public testing::Test cuda::unique_ptr dir_cls_output_d_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #endif // TEST_POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.cpp b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp similarity index 95% rename from perception/lidar_transfusion/test/test_preprocess_kernel.cpp rename to perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp index c1c2a824f0f53..c4a1c37b4ff77 100644 --- a/perception/lidar_transfusion/test/test_preprocess_kernel.cpp +++ b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.cpp @@ -14,9 +14,9 @@ #include "test_preprocess_kernel.hpp" -#include "lidar_transfusion/cuda_utils.hpp" +#include "autoware/lidar_transfusion/cuda_utils.hpp" -#include +#include #include #include @@ -25,22 +25,24 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void PreprocessKernelTest::SetUp() { cudaStreamCreate(&stream_); + auto cloud_capacity = 2000000; auto voxels_num = std::vector{1, 3, 5}; auto point_cloud_range = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; auto voxel_size = std::vector{0.3, 0.3, 8.0}; + auto num_proposals = 500; auto score_threshold = 0.2f; auto circle_nms_dist_threshold = 0.5f; auto yaw_norm_thresholds = std::vector{0.5, 0.5, 0.5}; config_ptr_ = std::make_unique( - voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, - score_threshold); + cloud_capacity, voxels_num, point_cloud_range, voxel_size, num_proposals, + circle_nms_dist_threshold, yaw_norm_thresholds, score_threshold); pre_ptr_ = std::make_unique(*config_ptr_, stream_); voxel_features_size_ = config_ptr_->max_voxels_ * config_ptr_->max_num_points_per_pillar_ * @@ -256,7 +258,7 @@ TEST_F(PreprocessKernelTest, VoxelOverflowTest) EXPECT_EQ(config_ptr_->max_voxels_, params_input); } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion int main(int argc, char ** argv) { diff --git a/perception/lidar_transfusion/test/test_preprocess_kernel.hpp b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.hpp similarity index 87% rename from perception/lidar_transfusion/test/test_preprocess_kernel.hpp rename to perception/autoware_lidar_transfusion/test/test_preprocess_kernel.hpp index 024fae5eae4b1..260e4261c3c42 100644 --- a/perception/lidar_transfusion/test/test_preprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/test/test_preprocess_kernel.hpp @@ -15,14 +15,14 @@ #ifndef TEST_PREPROCESS_KERNEL_HPP_ #define TEST_PREPROCESS_KERNEL_HPP_ -#include -#include +#include +#include #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class PreprocessKernelTest : public testing::Test @@ -45,6 +45,6 @@ class PreprocessKernelTest : public testing::Test cuda::unique_ptr voxel_idxs_d_{nullptr}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #endif // TEST_PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/test/test_ros_utils.cpp b/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp similarity index 79% rename from perception/lidar_transfusion/test/test_ros_utils.cpp rename to perception/autoware_lidar_transfusion/test/test_ros_utils.cpp index 15541e6353b42..ac9c4ca0378cb 100644 --- a/perception/lidar_transfusion/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_transfusion/test/test_ros_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lidar_transfusion/ros_utils.hpp" +#include "autoware/lidar_transfusion/ros_utils.hpp" #include @@ -23,7 +23,7 @@ TEST(TestSuite, box3DToDetectedObject) // Test case 1: Test with valid label { - lidar_transfusion::Box3D box3d; + autoware::lidar_transfusion::Box3D box3d; box3d.label = 0; // CAR box3d.score = 0.8f; box3d.x = 1.0; @@ -35,7 +35,7 @@ TEST(TestSuite, box3DToDetectedObject) box3d.yaw = 0.5; autoware_perception_msgs::msg::DetectedObject obj; - lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + autoware::lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); EXPECT_EQ( @@ -53,12 +53,12 @@ TEST(TestSuite, box3DToDetectedObject) // Test case 2: Test with invalid label { - lidar_transfusion::Box3D box3d; + autoware::lidar_transfusion::Box3D box3d; box3d.score = 0.5f; box3d.label = 10; // Invalid autoware_perception_msgs::msg::DetectedObject obj; - lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); + autoware::lidar_transfusion::box3DToDetectedObject(box3d, class_names, obj); EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); EXPECT_EQ( @@ -72,28 +72,28 @@ TEST(TestSuite, box3DToDetectedObject) TEST(TestSuite, getSemanticType) { EXPECT_EQ( - lidar_transfusion::getSemanticType("CAR"), + autoware::lidar_transfusion::getSemanticType("CAR"), autoware_perception_msgs::msg::ObjectClassification::CAR); EXPECT_EQ( - lidar_transfusion::getSemanticType("TRUCK"), + autoware::lidar_transfusion::getSemanticType("TRUCK"), autoware_perception_msgs::msg::ObjectClassification::TRUCK); EXPECT_EQ( - lidar_transfusion::getSemanticType("BUS"), + autoware::lidar_transfusion::getSemanticType("BUS"), autoware_perception_msgs::msg::ObjectClassification::BUS); EXPECT_EQ( - lidar_transfusion::getSemanticType("TRAILER"), + autoware::lidar_transfusion::getSemanticType("TRAILER"), autoware_perception_msgs::msg::ObjectClassification::TRAILER); EXPECT_EQ( - lidar_transfusion::getSemanticType("MOTORCYCLE"), + autoware::lidar_transfusion::getSemanticType("MOTORCYCLE"), autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); EXPECT_EQ( - lidar_transfusion::getSemanticType("BICYCLE"), + autoware::lidar_transfusion::getSemanticType("BICYCLE"), autoware_perception_msgs::msg::ObjectClassification::BICYCLE); EXPECT_EQ( - lidar_transfusion::getSemanticType("PEDESTRIAN"), + autoware::lidar_transfusion::getSemanticType("PEDESTRIAN"), autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN); EXPECT_EQ( - lidar_transfusion::getSemanticType("UNKNOWN"), + autoware::lidar_transfusion::getSemanticType("UNKNOWN"), autoware_perception_msgs::msg::ObjectClassification::UNKNOWN); } diff --git a/perception/lidar_transfusion/test/test_voxel_generator.cpp b/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp similarity index 96% rename from perception/lidar_transfusion/test/test_voxel_generator.cpp rename to perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp index 2673a341b3721..5a60c206314bf 100644 --- a/perception/lidar_transfusion/test/test_voxel_generator.cpp +++ b/perception/autoware_lidar_transfusion/test/test_voxel_generator.cpp @@ -14,15 +14,15 @@ #include "test_voxel_generator.hpp" +#include "autoware/lidar_transfusion/transfusion_config.hpp" +#include "autoware/lidar_transfusion/utils.hpp" #include "gtest/gtest.h" -#include "lidar_transfusion/transfusion_config.hpp" -#include "lidar_transfusion/utils.hpp" #include #include "sensor_msgs/point_cloud2_iterator.hpp" -namespace lidar_transfusion +namespace autoware::lidar_transfusion { void VoxelGeneratorTest::SetUp() @@ -35,15 +35,17 @@ void VoxelGeneratorTest::SetUp() delta_pointcloud_x_ = 1.0; points_per_pointcloud_ = 300; + cloud_capacity_ = 2000000; voxels_num_ = std::vector{5000, 30000, 60000}; point_cloud_range_ = std::vector{-76.8, -76.8, -3.0, 76.8, 76.8, 5.0}; voxel_size_ = std::vector{0.3, 0.3, 8.0}; + num_proposals_ = 500; score_threshold_ = 0.2f; circle_nms_dist_threshold_ = 0.5f; yaw_norm_thresholds_ = std::vector{0.5, 0.5, 0.5}; config_ptr_ = std::make_unique( - voxels_num_, point_cloud_range_, voxel_size_, circle_nms_dist_threshold_, yaw_norm_thresholds_, - score_threshold_); + cloud_capacity_, voxels_num_, point_cloud_range_, voxel_size_, num_proposals_, + circle_nms_dist_threshold_, yaw_norm_thresholds_, score_threshold_); cloud1_ = std::make_unique(); cloud2_ = std::make_unique(); @@ -274,7 +276,7 @@ TEST_F(VoxelGeneratorTest, TwoFrames) EXPECT_TRUE(std::isnan(points[i])); } } -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion int main(int argc, char ** argv) { diff --git a/perception/lidar_transfusion/test/test_voxel_generator.hpp b/perception/autoware_lidar_transfusion/test/test_voxel_generator.hpp similarity index 89% rename from perception/lidar_transfusion/test/test_voxel_generator.hpp rename to perception/autoware_lidar_transfusion/test/test_voxel_generator.hpp index eccbe6412d183..49346e69054f3 100644 --- a/perception/lidar_transfusion/test/test_voxel_generator.hpp +++ b/perception/autoware_lidar_transfusion/test/test_voxel_generator.hpp @@ -15,7 +15,7 @@ #ifndef TEST_VOXEL_GENERATOR_HPP_ #define TEST_VOXEL_GENERATOR_HPP_ -#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace lidar_transfusion +namespace autoware::lidar_transfusion { class VoxelGeneratorTest : public testing::Test @@ -51,15 +51,17 @@ class VoxelGeneratorTest : public testing::Test double delta_pointcloud_x_{}; std::size_t points_per_pointcloud_{}; + std::size_t cloud_capacity_{}; std::vector voxels_num_{}; std::vector point_cloud_range_{}; std::vector voxel_size_{}; + std::size_t num_proposals_{}; float circle_nms_dist_threshold_{}; std::vector yaw_norm_thresholds_{}; float score_threshold_{}; cudaStream_t stream_{}; }; -} // namespace lidar_transfusion +} // namespace autoware::lidar_transfusion #endif // TEST_VOXEL_GENERATOR_HPP_ diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index ce1c587cc3b01..1c677c9d0f6e4 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -222,6 +223,9 @@ class MapBasedPredictionNode : public rclcpp::Node bool remember_lost_crosswalk_users_; std::unique_ptr published_time_publisher_; + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + mutable autoware::universe_utils::TimeKeeper time_keeper_; // Member Functions void mapCallback(const LaneletMapBin::ConstSharedPtr msg); 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 478e8d6f72dd3..b1402724e943d 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 @@ -863,6 +863,11 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ published_time_publisher_ = std::make_unique(this); + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); @@ -922,6 +927,8 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject( void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -938,6 +945,8 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg void MapBasedPredictionNode::trafficSignalsCallback( const TrafficLightGroupArray::ConstSharedPtr msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + traffic_signal_id_map_.clear(); for (const auto & signal : msg->traffic_light_groups) { traffic_signal_id_map_[signal.traffic_light_group_id] = signal; @@ -946,6 +955,8 @@ void MapBasedPredictionNode::trafficSignalsCallback( void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + stop_watch_ptr_->toc("processing_time", true); // take traffic_signal @@ -1236,6 +1247,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt void MapBasedPredictionNode::updateCrosswalkUserHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + CrosswalkUserData crosswalk_user_data; crosswalk_user_data.header = header; crosswalk_user_data.tracked_object = object; @@ -1251,6 +1264,8 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory( std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( const std::string & object_id, std::unordered_map & current_users) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const auto known_match_opt = [&]() -> std::optional { if (!known_matches_.count(object_id)) { return std::nullopt; @@ -1308,6 +1323,8 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + lanelet::BasicLineString2d predicted_path_ls; for (const auto & p : predicted_path.path) predicted_path_ls.emplace_back(p.position.x, p.position.y); @@ -1325,6 +1342,8 @@ bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predict bool MapBasedPredictionNode::doesPathCrossFence( const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + // check whether the predicted path cross with fence for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) { for (size_t j = 0; j < fence_line.size() - 1; ++j) { @@ -1353,6 +1372,8 @@ bool MapBasedPredictionNode::isIntersecting( PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const TrackedObject & object) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + auto predicted_object = convertToPredictedObject(object); { PredictedPath predicted_path = @@ -1485,6 +1506,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( void MapBasedPredictionNode::updateObjectData(TrackedObject & object) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if ( object.kinematics.orientation_availability == autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { @@ -1535,6 +1558,8 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) void MapBasedPredictionNode::removeStaleTrafficLightInfo( const TrackedObjects::ConstSharedPtr in_objects) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { const bool isDisappeared = std::none_of( in_objects->objects.begin(), in_objects->objects.end(), @@ -1551,6 +1576,8 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + // obstacle point lanelet::BasicPoint2d search_point( object.kinematics.pose_with_covariance.pose.position.x, @@ -1641,6 +1668,8 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob bool MapBasedPredictionNode::checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { return false; @@ -1687,6 +1716,8 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( float MapBasedPredictionNode::calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; // compute yaw difference between the object and lane @@ -1722,6 +1753,8 @@ void MapBasedPredictionNode::updateRoadUsersHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); @@ -1763,6 +1796,8 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time, const double time_horizon) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); @@ -1925,6 +1960,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + // calculate maneuver const auto current_maneuver = [&]() { if (lane_change_detection_method_ == "time_to_change_lane") { @@ -1975,6 +2012,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( const TrackedObject & object, const LaneletData & current_lanelet_data, const double /*object_detected_time*/) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { @@ -2046,6 +2085,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( const TrackedObject & object, const LaneletData & current_lanelet_data, const double /*object_detected_time*/) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { @@ -2189,6 +2230,8 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( double MapBasedPredictionNode::calcRightLateralOffset( const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::vector boundary_path(boundary_line.size()); for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); @@ -2208,6 +2251,8 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return; @@ -2232,6 +2277,8 @@ void MapBasedPredictionNode::addReferencePaths( const Maneuver & maneuver, std::vector & reference_paths, const double speed_limit) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if (!candidate_paths.empty()) { updateFuturePossibleLanelets(object, candidate_paths); const auto converted_paths = convertPathType(candidate_paths); @@ -2251,6 +2298,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( const lanelet::routing::LaneletPaths & right_paths, const lanelet::routing::LaneletPaths & center_paths) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + float left_lane_change_probability = 0.0; float right_lane_change_probability = 0.0; float lane_follow_probability = 0.0; @@ -2313,6 +2362,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( std::vector MapBasedPredictionNode::convertPathType( const lanelet::routing::LaneletPaths & paths) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::vector converted_paths; for (const auto & path : paths) { PosePath converted_path; @@ -2385,6 +2436,8 @@ bool MapBasedPredictionNode::isDuplicated( const std::pair & target_lanelet, const LaneletsData & lanelets_data) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const double CLOSE_LANELET_THRESHOLD = 0.1; for (const auto & lanelet_data : lanelets_data) { const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); @@ -2402,6 +2455,8 @@ bool MapBasedPredictionNode::isDuplicated( bool MapBasedPredictionNode::isDuplicated( const PredictedPath & predicted_path, const std::vector & predicted_paths) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const double CLOSE_PATH_THRESHOLD = 0.1; for (const auto & prev_predicted_path : predicted_paths) { const auto prev_path_end = prev_predicted_path.path.back().position; @@ -2418,6 +2473,8 @@ bool MapBasedPredictionNode::isDuplicated( std::optional MapBasedPredictionNode::getTrafficSignalId( const lanelet::ConstLanelet & way_lanelet) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const auto traffic_light_reg_elems = way_lanelet.regulatoryElementsAs(); if (traffic_light_reg_elems.empty()) { @@ -2434,6 +2491,8 @@ std::optional MapBasedPredictionNode::getTrafficSignalId( std::optional MapBasedPredictionNode::getTrafficSignalElement( const lanelet::Id & id) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if (traffic_signal_id_map_.count(id) != 0) { const auto & signal_elements = traffic_signal_id_map_.at(id).elements; if (signal_elements.size() > 1) { @@ -2450,6 +2509,8 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, const lanelet::Id & signal_id) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const auto signal_color = [&] { const auto elem_opt = getTrafficSignalElement(signal_id); return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN; 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..afb1598645733 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/autoware_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/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} +{{ json_to_markdown("perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json") }} + +#### Simulation parameters + +{{ json_to_markdown("perception/autoware_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 75% 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 index 0f987fc49ee7c..a9893c4117841 100644 --- 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 @@ -32,7 +32,15 @@ class SSP : public GnnSolverInterface void maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) override; + std::unordered_map * reverse_assignment) override + { + const bool sparse_cost = true; + maximizeLinearAssignment(cost, direct_assignment, reverse_assignment, sparse_cost); + } + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment, const bool sparse_cost = true); }; } // namespace gnn_solver 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 84333fe8a1ff6..e559a59c4bece 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 @@ -47,12 +47,8 @@ struct ResidualEdge void SSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) + std::unordered_map * reverse_assignment, const bool sparse_cost) { - // NOTE: Need to set as default arguments - bool sparse_cost = true; - // bool sparse_cost = false; - // Hyperparameters // double MAX_COST = 6; const double MAX_COST = 10; 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 99% rename from perception/multi_object_tracker/src/debugger/debug_object.cpp rename to perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index 58b7dfc5cef48..59f50bd2e9f0b 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -51,7 +51,7 @@ int32_t uuidToInt(const boost::uuids::uuid & uuid) namespace autoware::multi_object_tracker { -TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) +TrackerObjectDebugger::TrackerObjectDebugger(const std::string & frame_id) { // set frame id frame_id_ = frame_id; @@ -173,7 +173,7 @@ void TrackerObjectDebugger::process() } void TrackerObjectDebugger::draw( - const std::vector> object_data_groups, + const std::vector> & object_data_groups, visualization_msgs::msg::MarkerArray & marker_array) const { // initialize markers 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 95% rename from perception/multi_object_tracker/src/debugger/debug_object.hpp rename to perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index ed4c556959861..c77b465fb2bc9 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -65,7 +65,7 @@ struct ObjectData class TrackerObjectDebugger { public: - explicit TrackerObjectDebugger(std::string frame_id); + explicit TrackerObjectDebugger(const std::string & frame_id); private: bool is_initialized_{false}; @@ -96,7 +96,7 @@ class TrackerObjectDebugger void reset(); void draw( - const std::vector> object_data_groups, + const std::vector> & object_data_groups, visualization_msgs::msg::MarkerArray & marker_array) const; void process(); void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; 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 59% 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..b39acb29404ef 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 @@ -30,8 +30,16 @@ class SSP : public GnnSolverInterface void maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) override; + std::unordered_map * reverse_assignment) override + { + const bool sparse_cost = true; + maximizeLinearAssignment(cost, direct_assignment, reverse_assignment, sparse_cost); + } + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment, const bool sparse_cost = true); }; } // 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 92% 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 d11f58bc5a857..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 @@ -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 98% 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..2738ba8f0346b 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 @@ -45,12 +45,8 @@ struct ResidualEdge void SSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) + std::unordered_map * reverse_assignment, const bool sparse_cost) { - // NOTE: Need to set as default arguments - bool sparse_cost = true; - // bool sparse_cost = false; - // Hyperparameters // double MAX_COST = 6; const double MAX_COST = 10; 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 98% 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..cd5e96d2374fc 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" @@ -36,8 +36,8 @@ bool isUnknownObjectOverlapped( const autoware_perception_msgs::msg::DetectedObject & unknown_object, const autoware_perception_msgs::msg::DetectedObject & known_object, const double precision_threshold, const double recall_threshold, - std::map distance_threshold_map, - const std::map generalized_iou_threshold_map) + const std::map & distance_threshold_map, + const std::map & generalized_iou_threshold_map) { const double generalized_iou_threshold = generalized_iou_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); diff --git a/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp index a10692faeff1e..ddc7171b4d57d 100644 --- a/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp +++ b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.cpp @@ -65,14 +65,14 @@ ObjectVelocitySplitterNode::ObjectVelocitySplitterNode(const rclcpp::NodeOptions pub_low_speed_objects_ = create_publisher("~/output/low_speed_objects", 1); } -void ObjectVelocitySplitterNode::onObjects(const DetectedObjects::ConstSharedPtr objects_data_) +void ObjectVelocitySplitterNode::onObjects(const DetectedObjects::ConstSharedPtr msg) { DetectedObjects high_speed_objects; DetectedObjects low_speed_objects; - high_speed_objects.header = objects_data_->header; - low_speed_objects.header = objects_data_->header; + high_speed_objects.header = msg->header; + low_speed_objects.header = msg->header; - for (const auto & object : objects_data_->objects) { + for (const auto & object : msg->objects) { if ( std::abs(autoware::universe_utils::calcNorm( object.kinematics.twist_with_covariance.twist.linear)) < node_param_.velocity_threshold) { diff --git a/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp index ca0a9a9e65f3c..44543298efc10 100644 --- a/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp +++ b/perception/autoware_object_velocity_splitter/src/object_velocity_splitter_node.hpp @@ -46,9 +46,6 @@ class ObjectVelocitySplitterNode : public rclcpp::Node // Callback void onObjects(const DetectedObjects::ConstSharedPtr msg); - // Data Buffer - DetectedObjects::ConstSharedPtr objects_data_{}; - // Publisher rclcpp::Publisher::SharedPtr pub_high_speed_objects_{}; rclcpp::Publisher::SharedPtr pub_low_speed_objects_{}; diff --git a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt b/perception/autoware_occupancy_grid_map_outlier_filter/CMakeLists.txt similarity index 91% rename from perception/occupancy_grid_map_outlier_filter/CMakeLists.txt rename to perception/autoware_occupancy_grid_map_outlier_filter/CMakeLists.txt index 9754cea458577..0fc73d43e54c9 100644 --- a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt +++ b/perception/autoware_occupancy_grid_map_outlier_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(occupancy_grid_map_outlier_filter) +project(autoware_occupancy_grid_map_outlier_filter) find_package(autoware_cmake REQUIRED) autoware_package() @@ -41,6 +41,6 @@ endif() # -- Occupancy Grid Map Outlier Filter -- rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" - EXECUTABLE occupancy_grid_map_outlier_filter_node) + EXECUTABLE ${PROJECT_NAME}_node) ament_auto_package(INSTALL_TO_SHARE) diff --git a/perception/occupancy_grid_map_outlier_filter/README.md b/perception/autoware_occupancy_grid_map_outlier_filter/README.md similarity index 99% rename from perception/occupancy_grid_map_outlier_filter/README.md rename to perception/autoware_occupancy_grid_map_outlier_filter/README.md index fd064af19902f..28974edf65053 100644 --- a/perception/occupancy_grid_map_outlier_filter/README.md +++ b/perception/autoware_occupancy_grid_map_outlier_filter/README.md @@ -1,4 +1,4 @@ -# occupancy_grid_map_outlier_filter +# autoware_occupancy_grid_map_outlier_filter ## Purpose diff --git a/perception/occupancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg b/perception/autoware_occupancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg similarity index 100% rename from perception/occupancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg rename to perception/autoware_occupancy_grid_map_outlier_filter/image/occupancy_grid_map_outlier_filter.drawio.svg diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/autoware_occupancy_grid_map_outlier_filter/package.xml similarity index 92% rename from perception/occupancy_grid_map_outlier_filter/package.xml rename to perception/autoware_occupancy_grid_map_outlier_filter/package.xml index e92e5edb4b084..c39ebdad4b283 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/autoware_occupancy_grid_map_outlier_filter/package.xml @@ -1,7 +1,7 @@ - occupancy_grid_map_outlier_filter + autoware_occupancy_grid_map_outlier_filter 0.1.0 The ROS 2 occupancy_grid_map_outlier_filter package amc-nu @@ -18,13 +18,13 @@ autoware_cmake autoware_lanelet2_extension + autoware_pointcloud_preprocessor autoware_universe_utils libpcl-all-dev message_filters nav_msgs pcl_conversions pcl_ros - pointcloud_preprocessor rclcpp rclcpp_components sensor_msgs diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp similarity index 100% rename from perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp rename to perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp similarity index 98% rename from perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp rename to perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp index e9402d6416cbd..b7ec3ffeac6df 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.hpp @@ -15,8 +15,8 @@ #ifndef OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ #define OCCUPANCY_GRID_MAP_OUTLIER_FILTER_NODE_HPP_ +#include "autoware/pointcloud_preprocessor/filter.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" -#include "pointcloud_preprocessor/filter.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt similarity index 98% rename from perception/probabilistic_occupancy_grid_map/CMakeLists.txt rename to perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt index d01ef157e21b4..28342ecb5bcff 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(probabilistic_occupancy_grid_map) +project(autoware_probabilistic_occupancy_grid_map) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/autoware_probabilistic_occupancy_grid_map/README.md b/perception/autoware_probabilistic_occupancy_grid_map/README.md new file mode 100644 index 0000000000000..575411bcbd220 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/README.md @@ -0,0 +1,65 @@ +# autoware_probabilistic_occupancy_grid_map + +## Purpose + +This package outputs the probability of having an obstacle as occupancy grid map. +![pointcloud_based_occupancy_grid_map_sample_image](./image/pointcloud_based_occupancy_grid_map_sample_image.gif) + +## References/External links + +- [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md) +- [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md) +- [Grid map fusion](synchronized_grid_map_fusion.md) + +## Settings + +Occupancy grid map is generated on `map_frame`, and grid orientation is fixed. + +You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which means sensor origin and gridmap origin respectively. Especially, set your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) as a `scan_origin_frame` would result in better performance. + +![image_for_frame_parameter_visualization](./image/gridmap_frame_settings.drawio.svg) + +### Parameters + +{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json") }} +{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/grid_map.schema.json") }} +{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json") }} +{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json") }} + +### Downsample input pointcloud(Optional) + +If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used. This feature is currently only for the pointcloud based occupancy grid map. + +- pointcloud_based_occupancy_grid_map method + +```yaml +# downsampled raw and obstacle pointcloud +/perception/occupancy_grid_map/obstacle/downsample/pointcloud +/perception/occupancy_grid_map/raw/downsample/pointcloud +``` + +- multi_lidar_pointcloud_based_point_cloud + +```yaml +# downsampled raw and obstacle pointcloud +/perception/occupancy_grid_map/obstacle/downsample/pointcloud +/perception/occupancy_grid_map//raw/downsample/pointcloud +``` + +### Test + +This package provides unit tests using `gtest`. +You can run the test by the following command. + +```bash +colcon test --packages-select autoware_probabilistic_occupancy_grid_map --event-handlers console_direct+ +``` + +Test contains the following. + +- Unit test for cost value conversion function +- Unit test for utility functions +- Unit test for occupancy grid map fusion functions +- Input/Output test for pointcloud based occupancy grid map diff --git a/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml b/perception/autoware_probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml similarity index 100% rename from perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml rename to perception/autoware_probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/config/grid_map_param.yaml b/perception/autoware_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/autoware_probabilistic_occupancy_grid_map/config/grid_map.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml b/perception/autoware_probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml similarity index 100% rename from perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml rename to perception/autoware_probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/perception/autoware_probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml similarity index 100% rename from perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml rename to perception/autoware_probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/autoware_probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml similarity index 100% rename from perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml rename to perception/autoware_probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml b/perception/autoware_probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml similarity index 100% rename from perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml rename to perception/autoware_probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml diff --git a/perception/probabilistic_occupancy_grid_map/image/bresenham.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/bresenham.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/bresenham.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/bresenham.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png b/perception/autoware_probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png rename to perception/autoware_probabilistic_occupancy_grid_map/image/laserscan_based_occupancy_grid_map_sample_image.png diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_bev.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif b/perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif rename to perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_sample_image.gif diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_1st.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd_projection.drawio.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd_projection.drawio.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd_projection.drawio.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_2nd_projection.drawio.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/pointcloud_based_occupancy_grid_map_side_view_3rd.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg diff --git a/perception/probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg b/perception/autoware_probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg similarity index 100% rename from perception/probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg rename to perception/autoware_probabilistic_occupancy_grid_map/image/update_with_pointcloud.svg diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/fusion_policy/fusion_policy.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp diff --git a/perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp diff --git a/perception/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md b/perception/autoware_probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md similarity index 100% rename from perception/probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md rename to perception/autoware_probabilistic_occupancy_grid_map/laserscan-based-occupancy-grid-map.md diff --git a/perception/probabilistic_occupancy_grid_map/launch/debug.launch.xml b/perception/autoware_probabilistic_occupancy_grid_map/launch/debug.launch.xml similarity index 69% rename from perception/probabilistic_occupancy_grid_map/launch/debug.launch.xml rename to perception/autoware_probabilistic_occupancy_grid_map/launch/debug.launch.xml index 72edb00ea606a..20a1b2b7910e6 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/debug.launch.xml +++ b/perception/autoware_probabilistic_occupancy_grid_map/launch/debug.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/autoware_probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py similarity index 96% rename from perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py rename to perception/autoware_probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 1edc538b3de3f..1a73518531217 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/autoware_probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -84,7 +84,7 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ComposableNode( - package="probabilistic_occupancy_grid_map", + package="autoware_probabilistic_occupancy_grid_map", plugin="autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ @@ -162,13 +162,13 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output/stixel", "virtual_scan/stixel"), add_launch_arg( "param_file", - get_package_share_directory("probabilistic_occupancy_grid_map") + get_package_share_directory("autoware_probabilistic_occupancy_grid_map") + "/config/laserscan_based_occupancy_grid_map.param.yaml", ), add_launch_arg("updater_type", "binary_bayes_filter"), add_launch_arg( "updater_param_file", - get_package_share_directory("probabilistic_occupancy_grid_map") + get_package_share_directory("autoware_probabilistic_occupancy_grid_map") + "/config/updater.param.yaml", ), add_launch_arg("input_obstacle_pointcloud", "false"), diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/autoware_probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py similarity index 95% rename from perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py rename to perception/autoware_probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index ef40839d2a5aa..f8e050c7e8206 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/autoware_probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -98,7 +98,7 @@ def get_downsample_filter_node(setting: dict) -> ComposableNode: voxel_size = setting["voxel_size"] node_name = setting["node_name"] return ComposableNode( - package="pointcloud_preprocessor", + package="autoware_pointcloud_preprocessor", plugin=plugin_str, name=node_name, remappings=[ @@ -125,7 +125,7 @@ def get_downsample_preprocess_nodes(voxel_size: float, raw_pointcloud_topics: li ] # `/sensing/lidar/top/pointcloud` -> `top processed_pointcloud_topic: str = frame_name + "/raw/downsample/pointcloud" raw_settings = { - "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", + "plugin": "autoware::pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", "node_name": "raw_pc_downsample_filter_" + frame_name, "input_topic": raw_pointcloud_topic, "output_topic": processed_pointcloud_topic, @@ -133,7 +133,7 @@ def get_downsample_preprocess_nodes(voxel_size: float, raw_pointcloud_topics: li } nodes.append(get_downsample_filter_node(raw_settings)) obstacle_settings = { - "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", + "plugin": "autoware::pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", "node_name": "obstacle_pc_downsample_filter", "input_topic": LaunchConfiguration("input/obstacle_pointcloud"), "output_topic": "/perception/occupancy_grid_map/obstacle/downsample/pointcloud", @@ -195,7 +195,7 @@ def launch_setup(context, *args, **kwargs): # generate composable node node = ComposableNode( - package="probabilistic_occupancy_grid_map", + package="autoware_probabilistic_occupancy_grid_map", plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", namespace=frame_name, @@ -218,7 +218,7 @@ def launch_setup(context, *args, **kwargs): # 2. launch occupancy grid map fusion node gridmap_fusion_node = [ ComposableNode( - package="probabilistic_occupancy_grid_map", + package="autoware_probabilistic_occupancy_grid_map", plugin="synchronized_grid_map_fusion::GridMapFusionNode", name="occupancy_grid_map_fusion_node", remappings=[ @@ -275,13 +275,13 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output", "occupancy_grid"), add_launch_arg( "multi_lidar_fusion_config_file", - get_package_share_directory("probabilistic_occupancy_grid_map") + get_package_share_directory("autoware_probabilistic_occupancy_grid_map") + "/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml", ), add_launch_arg("updater_type", "binary_bayes_filter"), add_launch_arg( "updater_param_file", - get_package_share_directory("probabilistic_occupancy_grid_map") + get_package_share_directory("autoware_probabilistic_occupancy_grid_map") + "/config/binary_bayes_filter_updater.param.yaml", ), set_container_executable, diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/autoware_probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py similarity index 93% rename from perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py rename to perception/autoware_probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index bb5ef025a7e47..2d70feeb0e357 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/autoware_probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -31,7 +31,7 @@ def get_downsample_filter_node(setting: dict) -> ComposableNode: voxel_size = setting["voxel_size"] node_name = setting["node_name"] return ComposableNode( - package="pointcloud_preprocessor", + package="autoware_pointcloud_preprocessor", plugin=plugin_str, name=node_name, remappings=[ @@ -51,14 +51,14 @@ def get_downsample_filter_node(setting: dict) -> ComposableNode: def get_downsample_preprocess_nodes(voxel_size: float) -> list: raw_settings = { - "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", + "plugin": "autoware::pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", "node_name": "raw_pc_downsample_filter", "input_topic": LaunchConfiguration("input/raw_pointcloud"), "output_topic": "raw/downsample/pointcloud", "voxel_size": voxel_size, } obstacle_settings = { - "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", + "plugin": "autoware::pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", "node_name": "obstacle_pc_downsample_filter", "input_topic": LaunchConfiguration("input/obstacle_pointcloud"), "output_topic": "obstacle/downsample/pointcloud", @@ -95,7 +95,7 @@ def launch_setup(context, *args, **kwargs): composable_nodes.append( ComposableNode( - package="probabilistic_occupancy_grid_map", + package="autoware_probabilistic_occupancy_grid_map", plugin="autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", name="occupancy_grid_map_node", remappings=[ @@ -173,13 +173,13 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output", "occupancy_grid"), add_launch_arg( "param_file", - get_package_share_directory("probabilistic_occupancy_grid_map") + get_package_share_directory("autoware_probabilistic_occupancy_grid_map") + "/config/pointcloud_based_occupancy_grid_map.param.yaml", ), add_launch_arg("updater_type", "binary_bayes_filter"), add_launch_arg( "updater_param_file", - get_package_share_directory("probabilistic_occupancy_grid_map") + get_package_share_directory("autoware_probabilistic_occupancy_grid_map") + "/config/binary_bayes_filter_updater.param.yaml", ), set_container_executable, diff --git a/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml b/perception/autoware_probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml similarity index 54% rename from perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml rename to perception/autoware_probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml index c2391141e9fa0..96f51a313475f 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml +++ b/perception/autoware_probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp similarity index 96% rename from perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index c8af97fcf3ba0..b940b932db0ef 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -160,7 +160,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( // First step: Initialize cells to the final point with freespace for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); + const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); BinInfo end_distance; if (raw_pointcloud_angle_bin.empty()) { @@ -175,8 +175,8 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( // Second step: Add unknown cell for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); - auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); + const auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); + const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); auto raw_distance_iter = raw_pointcloud_angle_bin.begin(); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { // Calculate next raw point from obstacle point @@ -232,7 +232,7 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( } // Third step: Overwrite occupied cell - for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { + for (const auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); diff --git a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp similarity index 99% rename from perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index f5f2c70ae6dfc..dd934dea26c55 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -285,7 +285,7 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( if (pub_debug_grid_) converter.addLayerFromCostmap2D(*this, "added_unknown", debug_grid_); // Third step: Overwrite occupied cell - for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { + for (const auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); diff --git a/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp similarity index 97% rename from perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp index 1e88b7a39fd14..6d058b3bef6eb 100644 --- a/perception/probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp @@ -36,7 +36,7 @@ double convertCharToProbability(const unsigned char & occupancy) std::vector convertProbabilityToChar(const std::vector & probabilities) { std::vector occupancies; - for (auto & probability : probabilities) { + for (const auto & probability : probabilities) { occupancies.push_back(convertProbabilityToChar(probability)); } return occupancies; @@ -45,7 +45,7 @@ std::vector convertProbabilityToChar(const std::vector & std::vector convertCharToProbability(const std::vector & occupancies) { std::vector probabilities; - for (auto & occupancy : occupancies) { + for (const auto & occupancy : occupancies) { probabilities.push_back(convertCharToProbability(occupancy)); } return probabilities; @@ -88,7 +88,7 @@ unsigned char overwriteFusion(const std::vector & occupancies) auto fusion_occupancy = 128; // unknown auto fusion_state = getApproximateState(fusion_occupancy); - for (auto & cell : occupancies) { + for (const auto & cell : occupancies) { auto state = getApproximateState(cell); if (state > fusion_state) { fusion_state = state; @@ -115,7 +115,7 @@ namespace log_odds_fusion double logOddsFusion(const std::vector & probabilities) { double log_odds = 0.0; - for (auto & probability : probabilities) { + for (const auto & probability : probabilities) { const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probability)); log_odds += std::log(p / (1.0 - p)); } @@ -252,7 +252,7 @@ struct dempsterShaferOccupancy double dempsterShaferFusion(const std::vector & probability) { dempsterShaferOccupancy result; // init with unknown - for (auto & p : probability) { + for (const auto & p : probability) { result = result + dempsterShaferOccupancy(p); } return result.getPignisticProbability(); diff --git a/perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp diff --git a/perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp diff --git a/perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/lib/utils/utils.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/autoware_probabilistic_occupancy_grid_map/package.xml similarity index 90% rename from perception/probabilistic_occupancy_grid_map/package.xml rename to perception/autoware_probabilistic_occupancy_grid_map/package.xml index d8647ae2ba744..e06eaf476c44d 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/autoware_probabilistic_occupancy_grid_map/package.xml @@ -1,7 +1,7 @@ - probabilistic_occupancy_grid_map + autoware_probabilistic_occupancy_grid_map 0.1.0 generate probabilistic occupancy grid map Yukihiro Saito @@ -31,18 +31,18 @@ tf2_ros tf2_sensor_msgs - pointcloud_preprocessor + autoware_pointcloud_preprocessor pointcloud_to_laserscan ament_cmake_gtest ament_lint_auto autoware_lint_common + autoware_pointcloud_preprocessor launch launch_ros launch_testing launch_testing_ament_cmake launch_testing_ros - pointcloud_preprocessor ament_cmake diff --git a/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md b/perception/autoware_probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md similarity index 99% rename from perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md rename to perception/autoware_probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md index d8d69ed7866b6..972c77c9e691e 100644 --- a/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md +++ b/perception/autoware_probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md @@ -130,7 +130,7 @@ In several places we have modified the external code written in BSD3 license. If `grid_map_type` is "OccupancyGridMapProjectiveBlindSpot" and `pub_debug_grid` is `true`, it is possible to check the each process of grid map generation by running ```shell -ros2 launch probabilistic_occupancy_grid_map debug.launch.xml +ros2 launch autoware_probabilistic_occupancy_grid_map debug.launch.xml ``` and visualizing the following occupancy grid map topics (which are listed in config/grid_map_param.yaml): diff --git a/perception/autoware_probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json b/perception/autoware_probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json new file mode 100644 index 0000000000000..d087095ce980b --- /dev/null +++ b/perception/autoware_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/autoware_probabilistic_occupancy_grid_map/schema/grid_map.schema.json b/perception/autoware_probabilistic_occupancy_grid_map/schema/grid_map.schema.json new file mode 100644 index 0000000000000..e874431ec2601 --- /dev/null +++ b/perception/autoware_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/autoware_probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json b/perception/autoware_probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..2b6befd1ae1fd --- /dev/null +++ b/perception/autoware_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/autoware_probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json b/perception/autoware_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/autoware_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/autoware_probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json b/perception/autoware_probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json new file mode 100644 index 0000000000000..e79c094820ec4 --- /dev/null +++ b/perception/autoware_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/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json b/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json new file mode 100644 index 0000000000000..14f4305f55de8 --- /dev/null +++ b/perception/autoware_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/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp similarity index 98% rename from perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index a271e17a1e566..0a78b10055dd0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -40,12 +40,6 @@ GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) throw std::runtime_error("The number of input topics must larger than 0."); } num_input_topics_ = input_topics_.size(); - if (num_input_topics_ < 1) { - RCLCPP_WARN( - this->get_logger(), "minimum num_input_topics_ is 1. current num_input_topics_ is %zu", - num_input_topics_); - num_input_topics_ = 1; - } if (num_input_topics_ > 12) { RCLCPP_WARN( this->get_logger(), "maximum num_input_topics_ is 12. current num_input_topics_ is %zu", diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp similarity index 99% rename from perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index c839f160aab9b..691575299a8c7 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -61,7 +61,7 @@ class GridMapFusionNode : public rclcpp::Node private: void onGridMap( const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, - const std::string & input_topic); + const std::string & topic_name); nav_msgs::msg::OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr( const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map); diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp diff --git a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/autoware_probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md similarity index 92% rename from perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md rename to perception/autoware_probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md index cb86ebe7eea85..98aff5cc3e9b7 100644 --- a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md +++ b/perception/autoware_probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -77,9 +77,9 @@ The minimum node launch will be like the following. - + - + @@ -90,7 +90,7 @@ The minimum node launch will be like the following. You need to generate OGMs in each sensor frame before achieving grid map fusion. -`probabilistic_occupancy_grid_map` package supports to generate OGMs for the each from the point cloud data. +`autoware_probabilistic_occupancy_grid_map` package supports to generate OGMs for the each from the point cloud data.
Example launch.xml (click to expand) @@ -107,7 +107,7 @@ You need to generate OGMs in each sensor frame before achieving grid map fusion. - + @@ -130,7 +130,7 @@ The minimum parameter for the OGM generation in each frame is shown in the follo
We recommend to use same `map_frame`, size and resolutions for the OGMs from synchronized sensors. -Also, remember to set `enable_single_frame_mode` and `filter_obstacle_pointcloud_by_raw_pointcloud` to `true` in the `probabilistic_occupancy_grid_map` package (you do not need to set these parameters if you use the above example config file). +Also, remember to set `enable_single_frame_mode` and `filter_obstacle_pointcloud_by_raw_pointcloud` to `true` in the `autoware_probabilistic_occupancy_grid_map` package (you do not need to set these parameters if you use the above example config file).
@@ -141,7 +141,7 @@ We prepared the launch file to run both OGM generation node and fusion node in [ You can include this launch file like the following. ```xml - + diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/autoware_probabilistic_occupancy_grid_map/test/cost_value_test.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/test/cost_value_test.cpp diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/autoware_probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp diff --git a/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py b/perception/autoware_probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py similarity index 99% rename from perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py rename to perception/autoware_probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py index 093811d4cc213..f8f043fdb016e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py +++ b/perception/autoware_probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py @@ -37,7 +37,7 @@ from sensor_msgs.msg import PointField from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster -PACKAGE_NAME = "probabilistic_occupancy_grid_map" +PACKAGE_NAME = "autoware_probabilistic_occupancy_grid_map" INPUT_TOPIC_RAW = "/raw" INPUT_TOPIC_OBSTACLE = "/obstacle" diff --git a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py b/perception/autoware_probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py similarity index 99% rename from perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py rename to perception/autoware_probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py index e73aea581433e..f2c79a837bc2e 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py +++ b/perception/autoware_probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py @@ -34,7 +34,7 @@ from rclpy.qos import QoSReliabilityPolicy from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster -PACKAGE_NAME = "probabilistic_occupancy_grid_map" +PACKAGE_NAME = "autoware_probabilistic_occupancy_grid_map" INPUT_TOPICS = ["/topic1", "/topic2"] DEBUG_OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/debug/single_frame_map" OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/output/occupancy_grid_map" diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/autoware_probabilistic_occupancy_grid_map/test/test_utils.cpp similarity index 100% rename from perception/probabilistic_occupancy_grid_map/test/test_utils.cpp rename to perception/autoware_probabilistic_occupancy_grid_map/test/test_utils.cpp diff --git a/perception/autoware_radar_crossing_objects_noise_filter/README.md b/perception/autoware_radar_crossing_objects_noise_filter/README.md index 846da0a0c11d1..553d932717442 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/README.md +++ b/perception/autoware_radar_crossing_objects_noise_filter/README.md @@ -24,7 +24,7 @@ Velocity estimation fails on static objects, resulting in ghost objects crossing - 2. Turning around by ego vehicle affect the output from radar. -When the ego vehicle turns around, the radars outputting at the object level sometimes fail to estimate the twist of objects correctly even if [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_tracks_msgs_converter) compensates by the ego vehicle twist. +When the ego vehicle turns around, the radars outputting at the object level sometimes fail to estimate the twist of objects correctly even if [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_radar_tracks_msgs_converter) compensates by the ego vehicle twist. So if an object detected by radars has circular motion viewing from base_link, it is likely that the speed is estimated incorrectly and that the object is a static object. The example is below figure. 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..b296f52e2c139 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,20 +99,20 @@ 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( + static bool isYawCorrect( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance, const double & yaw_threshold); - bool hasTwistCovariance(const TwistWithCovariance & twist_with_covariance); - Eigen::Vector2d toVector2d(const TwistWithCovariance & twist_with_covariance); - TwistWithCovariance toTwistWithCovariance(const Eigen::Vector2d & vector2d); + static bool hasTwistCovariance(const TwistWithCovariance & twist_with_covariance); + static Eigen::Vector2d toVector2d(const TwistWithCovariance & twist_with_covariance); + static TwistWithCovariance toTwistWithCovariance(const Eigen::Vector2d & vector2d); - double getTwistNorm(const Twist & twist); - LinearRing2d createObject2dWithMargin(const Point2d object_size, const double margin); + static double getTwistNorm(const Twist & twist); + static LinearRing2d createObject2dWithMargin(const Point2d object_size, const double margin); }; } // namespace autoware::radar_fusion_to_detected_object 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..0e66208d9f84e 100644 --- a/perception/autoware_radar_object_clustering/README.md +++ b/perception/autoware_radar_object_clustering/README.md @@ -2,7 +2,7 @@ This package contains a radar object clustering for [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.idl) input. -This package can make clustered objects from radar DetectedObjects, the objects which is converted from RadarTracks by [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_tracks_msgs_converter) and is processed by noise filter. +This package can make clustered objects from radar DetectedObjects, the objects which is converted from RadarTracks by [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_radar_tracks_msgs_converter) and is processed by noise filter. In other word, this package can combine multiple radar detections from one object into one and adjust class and size. ![radar_clustering](docs/radar_clustering.drawio.svg) @@ -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_clustering/src/radar_object_clustering_node.cpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp index b867d6a193ed7..4db4c578f9f89 100644 --- a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp +++ b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.cpp @@ -91,11 +91,11 @@ RadarObjectClusteringNode::RadarObjectClusteringNode(const rclcpp::NodeOptions & pub_objects_ = create_publisher("~/output/objects", 1); } -void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr objects_data_) +void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr msg) { DetectedObjects output_objects; - output_objects.header = objects_data_->header; - std::vector objects = objects_data_->objects; + output_objects.header = msg->header; + std::vector objects = msg->objects; std::vector used_flags(objects.size(), false); diff --git a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp index 13284a4151f49..ae78963c2ff39 100644 --- a/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp +++ b/perception/autoware_radar_object_clustering/src/radar_object_clustering_node.hpp @@ -54,9 +54,6 @@ class RadarObjectClusteringNode : public rclcpp::Node // Callback void onObjects(const DetectedObjects::ConstSharedPtr msg); - // Data Buffer - DetectedObjects::ConstSharedPtr objects_data_{}; - // Publisher rclcpp::Publisher::SharedPtr pub_objects_{}; diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp index c956e1ebd2f6d..4b415eee85ffb 100644 --- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp +++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/association/solver/ssp.hpp @@ -30,7 +30,15 @@ class SSP : public GnnSolverInterface void maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) override; + std::unordered_map * reverse_assignment) override + { + const bool sparse_cost = true; + maximizeLinearAssignment(cost, direct_assignment, reverse_assignment, sparse_cost); + } + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment, const bool sparse_cost = true); }; } // namespace autoware::radar_object_tracker::gnn_solver diff --git a/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp b/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp index d18e4f140e2ed..e869e5c9ab761 100644 --- a/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp +++ b/perception/autoware_radar_object_tracker/src/association/ssp/successive_shortest_path.cpp @@ -45,12 +45,8 @@ struct ResidualEdge void SSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) + std::unordered_map * reverse_assignment, const bool sparse_cost) { - // NOTE: Need to set as default arguments - bool sparse_cost = true; - // bool sparse_cost = false; - // Hyperparameters // double MAX_COST = 6; const double MAX_COST = 10; diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index 057a8653faea0..f30e4f984f8a1 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -280,11 +280,10 @@ bool ConstantTurnRateMotionTracker::predict(const double dt, KalmanFilter & ekf) // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. Eigen::MatrixXd Q_xy_local = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd Q_xy_global = Eigen::MatrixXd::Zero(2, 2); Q_xy_local << ekf_params_.q_cov_x, 0.0, 0.0, ekf_params_.q_cov_y; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(2, 2); R << cos(yaw), -sin(yaw), sin(yaw), cos(yaw); - Q_xy_global = R * Q_xy_local * R.transpose(); + Eigen::MatrixXd Q_xy_global = R * Q_xy_local * R.transpose(); Q.block<2, 2>(IDX::X, IDX::X) = Q_xy_global; Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw; @@ -366,23 +365,21 @@ bool ConstantTurnRateMotionTracker::measureWithPose( // covariance need to be rotated since it is in the vehicle coordinate system Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_position_covariance) { // switch noise covariance in polar coordinate or cartesian coordinate const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ ? depth * depth * ekf_params_.r_cov_x : ekf_params_.r_cov_y; Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate - Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); + R_block_list.push_back(RotationBaseLink * Rxy_local * RotationBaseLink.transpose()); } else { Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], object.kinematics.pose_with_covariance .covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate - Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); + R_block_list.push_back(RotationYaw * Rxy_local * RotationYaw.transpose()); } - R_block_list.push_back(Rxy); } // 2. add yaw measurement diff --git a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index a9f7fffff1156..dbadb3366ba0c 100644 --- a/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/autoware_radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -293,10 +293,6 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::VX, IDX::AX) = dt * acc_coeff; A(IDX::VY, IDX::AY) = dt * acc_coeff; - // Q: system noise - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd Q_local = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // system noise in local coordinate // we assume acceleration random walk model // @@ -311,7 +307,6 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const Eigen::VectorXd q_diag_vector = Eigen::VectorXd::Zero(ekf_params_.dim_x); q_diag_vector << ekf_params_.q_cov_x, ekf_params_.q_cov_y, ekf_params_.q_cov_vx, ekf_params_.q_cov_vy, ekf_params_.q_cov_ax, ekf_params_.q_cov_ay; - Q_local = q_diag_vector.asDiagonal(); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. @@ -326,7 +321,10 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const RotateCovMatrix.block<2, 2>(IDX::X, IDX::X) = R; RotateCovMatrix.block<2, 2>(IDX::VX, IDX::VX) = R; RotateCovMatrix.block<2, 2>(IDX::AX, IDX::AX) = R; - Q = RotateCovMatrix * Q_local * RotateCovMatrix.transpose(); + + // Q: system noise + Eigen::MatrixXd Q_local = q_diag_vector.asDiagonal(); + Eigen::MatrixXd Q = RotateCovMatrix * Q_local * RotateCovMatrix.transpose(); Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); @@ -396,23 +394,21 @@ bool LinearMotionTracker::measureWithPose( // covariance need to be rotated since it is in the vehicle coordinate system Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_position_covariance) { // switch noise covariance in polar coordinate or cartesian coordinate const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ ? depth * depth * ekf_params_.r_cov_x : ekf_params_.r_cov_y; Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate - Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); + R_block_list.push_back(RotationBaseLink * Rxy_local * RotationBaseLink.transpose()); } else { Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], object.kinematics.pose_with_covariance .covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate - Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); + R_block_list.push_back(RotationYaw * Rxy_local * RotationYaw.transpose()); } - R_block_list.push_back(Rxy); } // 2. add linear velocity measurement @@ -425,23 +421,19 @@ bool LinearMotionTracker::measureWithPose( // velocity is in the target vehicle coordinate system Eigen::MatrixXd Vxy_local = Eigen::MatrixXd::Zero(2, 1); - Eigen::MatrixXd Vxy = Eigen::MatrixXd::Zero(2, 1); Vxy_local << object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y; - Vxy = RotationYaw * Vxy_local; - Y_list.push_back(Vxy); + Y_list.push_back(RotationYaw * Vxy_local); Eigen::Matrix2d R_v_xy_local = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_twist_covariance) { R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; - R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose(); + R_block_list.push_back(RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose()); } else { R_v_xy_local << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X], 0, 0, object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; - R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); + R_block_list.push_back(RotationYaw * R_v_xy_local * RotationYaw.transpose()); } - R_block_list.push_back(R_v_xy); } // 3. sum up matrices diff --git a/perception/radar_tracks_msgs_converter/CMakeLists.txt b/perception/autoware_radar_tracks_msgs_converter/CMakeLists.txt similarity index 93% rename from perception/radar_tracks_msgs_converter/CMakeLists.txt rename to perception/autoware_radar_tracks_msgs_converter/CMakeLists.txt index 4ff3be1fa45a4..4a0e545c1c016 100644 --- a/perception/radar_tracks_msgs_converter/CMakeLists.txt +++ b/perception/autoware_radar_tracks_msgs_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_tracks_msgs_converter) +project(autoware_radar_tracks_msgs_converter) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/autoware_radar_tracks_msgs_converter/README.md similarity index 100% rename from perception/radar_tracks_msgs_converter/README.md rename to perception/autoware_radar_tracks_msgs_converter/README.md diff --git a/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml b/perception/autoware_radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml similarity index 100% rename from perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml rename to perception/autoware_radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/autoware_radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml similarity index 71% rename from perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml rename to perception/autoware_radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index 1aa4d51703b0c..d325778a21dbd 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/autoware_radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/autoware_radar_tracks_msgs_converter/package.xml similarity index 90% rename from perception/radar_tracks_msgs_converter/package.xml rename to perception/autoware_radar_tracks_msgs_converter/package.xml index 534c9a20fa0df..cb50b950a9240 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/autoware_radar_tracks_msgs_converter/package.xml @@ -1,9 +1,9 @@ - radar_tracks_msgs_converter + autoware_radar_tracks_msgs_converter 0.0.1 - radar_tracks_msgs_converter + autoware_radar_tracks_msgs_converter Satoshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json b/perception/autoware_radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json similarity index 100% rename from perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json rename to perception/autoware_radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp similarity index 99% rename from perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp rename to perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp index 5d45595733c4a..005138623427e 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp +++ b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp @@ -175,7 +175,7 @@ DetectedObjects RadarTracksMsgsConverterNode::convertTrackedObjectsToDetectedObj DetectedObjects detected_objects; detected_objects.header = objects.header; - for (auto & object : objects.objects) { + for (const auto & object : objects.objects) { DetectedObject detected_object; detected_object.existence_probability = 1.0; detected_object.shape = object.shape; @@ -224,7 +224,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() tracked_objects.header = radar_data_->header; tracked_objects.header.frame_id = node_param_.new_frame_id; - for (auto & radar_track : radar_data_->tracks) { + for (const auto & radar_track : radar_data_->tracks) { TrackedObject tracked_object; tracked_object.object_id = radar_track.uuid; diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp similarity index 100% rename from perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp rename to perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp diff --git a/perception/autoware_raindrop_cluster_filter/package.xml b/perception/autoware_raindrop_cluster_filter/package.xml index eea7dda76d4aa..46b7afd8c4a8d 100644 --- a/perception/autoware_raindrop_cluster_filter/package.xml +++ b/perception/autoware_raindrop_cluster_filter/package.xml @@ -12,9 +12,9 @@ ament_cmake autoware_cmake + autoware_detected_object_validation autoware_lanelet2_extension autoware_universe_utils - detected_object_validation message_filters nav_msgs pcl_conversions diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt similarity index 97% rename from perception/shape_estimation/CMakeLists.txt rename to perception/autoware_shape_estimation/CMakeLists.txt index 21517f8c0f766..b19f422c28d27 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/autoware_shape_estimation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(shape_estimation) +project(autoware_shape_estimation) find_package(autoware_cmake REQUIRED) autoware_package() @@ -54,7 +54,7 @@ target_include_directories(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME} ${SHAPE_ESTIMATION_DEPENDENCIES}) target_link_libraries(${PROJECT_NAME} - shape_estimation_lib + ${PROJECT_NAME}_lib ) rclcpp_components_register_node(${PROJECT_NAME} diff --git a/perception/shape_estimation/README.md b/perception/autoware_shape_estimation/README.md similarity index 94% rename from perception/shape_estimation/README.md rename to perception/autoware_shape_estimation/README.md index 9615b314f62c6..5f184c32f1805 100644 --- a/perception/shape_estimation/README.md +++ b/perception/autoware_shape_estimation/README.md @@ -1,4 +1,4 @@ -# shape_estimation +# autoware_shape_estimation ## Purpose @@ -36,7 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -{{ json_to_markdown("perception/autoware/shape_estimation/schema/shape_estimation.schema.json") }} +{{ json_to_markdown("perception/autoware_shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/autoware_shape_estimation/config/shape_estimation.param.yaml similarity index 100% rename from perception/shape_estimation/config/shape_estimation.param.yaml rename to perception/autoware_shape_estimation/config/shape_estimation.param.yaml diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/bicycle_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/bus_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/car_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/corrector_interface.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/no_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/reference_shape_size_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/trailer_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/truck_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/utils.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/corrector/vehicle_corrector.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/bus_filter.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/car_filter.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/filter.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/filter.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/filter.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/filter_interface.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/no_filter.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/trailer_filter.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/truck_filter.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/utils.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/filter/utils.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/filter/utils.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/model/bounding_box.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/model/convex_hull.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/model/cylinder.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/model/model.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/model/model.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/model/model.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/model/model_interface.hpp diff --git a/perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp similarity index 100% rename from perception/shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp rename to perception/autoware_shape_estimation/include/autoware/shape_estimation/shape_estimator.hpp diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml similarity index 62% rename from perception/shape_estimation/launch/shape_estimation.launch.xml rename to perception/autoware_shape_estimation/launch/shape_estimation.launch.xml index 88cea81412d97..816653013c6c8 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/perception/shape_estimation/lib/corrector/no_corrector.cpp b/perception/autoware_shape_estimation/lib/corrector/no_corrector.cpp similarity index 100% rename from perception/shape_estimation/lib/corrector/no_corrector.cpp rename to perception/autoware_shape_estimation/lib/corrector/no_corrector.cpp diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/autoware_shape_estimation/lib/corrector/utils.cpp similarity index 100% rename from perception/shape_estimation/lib/corrector/utils.cpp rename to perception/autoware_shape_estimation/lib/corrector/utils.cpp diff --git a/perception/shape_estimation/lib/filter/bus_filter.cpp b/perception/autoware_shape_estimation/lib/filter/bus_filter.cpp similarity index 100% rename from perception/shape_estimation/lib/filter/bus_filter.cpp rename to perception/autoware_shape_estimation/lib/filter/bus_filter.cpp diff --git a/perception/shape_estimation/lib/filter/car_filter.cpp b/perception/autoware_shape_estimation/lib/filter/car_filter.cpp similarity index 100% rename from perception/shape_estimation/lib/filter/car_filter.cpp rename to perception/autoware_shape_estimation/lib/filter/car_filter.cpp diff --git a/perception/shape_estimation/lib/filter/no_filter.cpp b/perception/autoware_shape_estimation/lib/filter/no_filter.cpp similarity index 100% rename from perception/shape_estimation/lib/filter/no_filter.cpp rename to perception/autoware_shape_estimation/lib/filter/no_filter.cpp diff --git a/perception/shape_estimation/lib/filter/trailer_filter.cpp b/perception/autoware_shape_estimation/lib/filter/trailer_filter.cpp similarity index 100% rename from perception/shape_estimation/lib/filter/trailer_filter.cpp rename to perception/autoware_shape_estimation/lib/filter/trailer_filter.cpp diff --git a/perception/shape_estimation/lib/filter/truck_filter.cpp b/perception/autoware_shape_estimation/lib/filter/truck_filter.cpp similarity index 100% rename from perception/shape_estimation/lib/filter/truck_filter.cpp rename to perception/autoware_shape_estimation/lib/filter/truck_filter.cpp diff --git a/perception/shape_estimation/lib/filter/utils.cpp b/perception/autoware_shape_estimation/lib/filter/utils.cpp similarity index 100% rename from perception/shape_estimation/lib/filter/utils.cpp rename to perception/autoware_shape_estimation/lib/filter/utils.cpp diff --git a/perception/shape_estimation/lib/model/bounding_box.cpp b/perception/autoware_shape_estimation/lib/model/bounding_box.cpp similarity index 100% rename from perception/shape_estimation/lib/model/bounding_box.cpp rename to perception/autoware_shape_estimation/lib/model/bounding_box.cpp diff --git a/perception/shape_estimation/lib/model/convex_hull.cpp b/perception/autoware_shape_estimation/lib/model/convex_hull.cpp similarity index 100% rename from perception/shape_estimation/lib/model/convex_hull.cpp rename to perception/autoware_shape_estimation/lib/model/convex_hull.cpp diff --git a/perception/shape_estimation/lib/model/cylinder.cpp b/perception/autoware_shape_estimation/lib/model/cylinder.cpp similarity index 100% rename from perception/shape_estimation/lib/model/cylinder.cpp rename to perception/autoware_shape_estimation/lib/model/cylinder.cpp diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/autoware_shape_estimation/lib/shape_estimator.cpp similarity index 100% rename from perception/shape_estimation/lib/shape_estimator.cpp rename to perception/autoware_shape_estimation/lib/shape_estimator.cpp diff --git a/perception/shape_estimation/package.xml b/perception/autoware_shape_estimation/package.xml similarity index 96% rename from perception/shape_estimation/package.xml rename to perception/autoware_shape_estimation/package.xml index 8eab542036177..ad226a5cc2f66 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/autoware_shape_estimation/package.xml @@ -1,7 +1,7 @@ - shape_estimation + autoware_shape_estimation 0.1.0 This package implements a shape estimation algorithm as a ROS 2 node Yukihiro Saito diff --git a/perception/shape_estimation/schema/shape_estimation.schema.json b/perception/autoware_shape_estimation/schema/shape_estimation.schema.json similarity index 100% rename from perception/shape_estimation/schema/shape_estimation.schema.json rename to perception/autoware_shape_estimation/schema/shape_estimation.schema.json diff --git a/perception/shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp similarity index 100% rename from perception/shape_estimation/src/shape_estimation_node.cpp rename to perception/autoware_shape_estimation/src/shape_estimation_node.cpp diff --git a/perception/shape_estimation/src/shape_estimation_node.hpp b/perception/autoware_shape_estimation/src/shape_estimation_node.hpp similarity index 100% rename from perception/shape_estimation/src/shape_estimation_node.hpp rename to perception/autoware_shape_estimation/src/shape_estimation_node.hpp diff --git a/perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp b/perception/autoware_shape_estimation/test/shape_estimation/test_shape_estimator.cpp similarity index 100% rename from perception/shape_estimation/test/shape_estimation/test_shape_estimator.cpp rename to perception/autoware_shape_estimation/test/shape_estimation/test_shape_estimator.cpp diff --git a/perception/shape_estimation/test/test_shape_estimation.cpp b/perception/autoware_shape_estimation/test/test_shape_estimation.cpp similarity index 100% rename from perception/shape_estimation/test/test_shape_estimation.cpp rename to perception/autoware_shape_estimation/test/test_shape_estimation.cpp diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/autoware_simple_object_merger/CMakeLists.txt similarity index 94% rename from perception/simple_object_merger/CMakeLists.txt rename to perception/autoware_simple_object_merger/CMakeLists.txt index 1a65bdf96050e..3a638d708d8a8 100644 --- a/perception/simple_object_merger/CMakeLists.txt +++ b/perception/autoware_simple_object_merger/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(simple_object_merger) +project(autoware_simple_object_merger) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/perception/simple_object_merger/README.md b/perception/autoware_simple_object_merger/README.md similarity index 92% rename from perception/simple_object_merger/README.md rename to perception/autoware_simple_object_merger/README.md index f69b31f70969e..a1e142b2bd85c 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/autoware_simple_object_merger/README.md @@ -1,4 +1,4 @@ -# simple_object_merger +# autoware_simple_object_merger This package can merge multiple topics of [autoware_perception_msgs/msg/DetectedObject](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_perception_msgs/msg/DetectedObject.msg) with low calculation cost. @@ -8,14 +8,14 @@ This package can merge multiple topics of [autoware_perception_msgs/msg/Detected [Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) is mainly used for merge process with DetectedObjects. There are 2 characteristics in `Object_merger`. First, `object_merger` solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. Second, `object_merger` can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. To merge 6 DetectedObjects topics, 6 `object_merger` nodes need to stand for now. -Therefore, `simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. +Therefore, `autoware_simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. The package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. ### Use case - Multiple radar detection -`Simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics, the pipeline for faraway detection with radar can be simpler. +`autoware_simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics, the pipeline for faraway detection with radar can be simpler. ### Limitation diff --git a/perception/simple_object_merger/config/simple_object_merger.param.yaml b/perception/autoware_simple_object_merger/config/simple_object_merger.param.yaml similarity index 100% rename from perception/simple_object_merger/config/simple_object_merger.param.yaml rename to perception/autoware_simple_object_merger/config/simple_object_merger.param.yaml diff --git a/perception/autoware_simple_object_merger/launch/simple_object_merger.launch.xml b/perception/autoware_simple_object_merger/launch/simple_object_merger.launch.xml new file mode 100644 index 0000000000000..1c4d0e7583139 --- /dev/null +++ b/perception/autoware_simple_object_merger/launch/simple_object_merger.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/perception/simple_object_merger/package.xml b/perception/autoware_simple_object_merger/package.xml similarity index 89% rename from perception/simple_object_merger/package.xml rename to perception/autoware_simple_object_merger/package.xml index 487155bffd102..cc9491d423a6f 100644 --- a/perception/simple_object_merger/package.xml +++ b/perception/autoware_simple_object_merger/package.xml @@ -1,8 +1,8 @@ - simple_object_merger + autoware_simple_object_merger 0.1.0 - simple_object_merger + autoware_simple_object_merger Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/perception/simple_object_merger/src/simple_object_merger_node.cpp b/perception/autoware_simple_object_merger/src/simple_object_merger_node.cpp similarity index 100% rename from perception/simple_object_merger/src/simple_object_merger_node.cpp rename to perception/autoware_simple_object_merger/src/simple_object_merger_node.cpp diff --git a/perception/simple_object_merger/src/simple_object_merger_node.hpp b/perception/autoware_simple_object_merger/src/simple_object_merger_node.hpp similarity index 100% rename from perception/simple_object_merger/src/simple_object_merger_node.hpp rename to perception/autoware_simple_object_merger/src/simple_object_merger_node.hpp diff --git a/perception/tensorrt_classifier/CMakeLists.txt b/perception/autoware_tensorrt_classifier/CMakeLists.txt similarity index 97% rename from perception/tensorrt_classifier/CMakeLists.txt rename to perception/autoware_tensorrt_classifier/CMakeLists.txt index 9f879a2c0c56d..88747a1e9240c 100644 --- a/perception/tensorrt_classifier/CMakeLists.txt +++ b/perception/autoware_tensorrt_classifier/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(tensorrt_classifier) +project(autoware_tensorrt_classifier) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -Wno-write-strings -fopenmp -Wall") diff --git a/perception/tensorrt_classifier/README.md b/perception/autoware_tensorrt_classifier/README.md similarity index 100% rename from perception/tensorrt_classifier/README.md rename to perception/autoware_tensorrt_classifier/README.md diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp similarity index 98% rename from perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp rename to perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp index b4d687c3c4fd9..088297f5bf796 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/calibrator.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/calibrator.hpp @@ -34,8 +34,8 @@ * DEALINGS IN THE SOFTWARE. */ -#ifndef TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ -#define TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ +#ifndef AUTOWARE__TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ +#define AUTOWARE__TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ #include "cuda_utils/cuda_check_error.hpp" #include "cuda_utils/cuda_unique_ptr.hpp" @@ -52,7 +52,7 @@ #include #include -namespace tensorrt_classifier +namespace autoware::tensorrt_classifier { class ImageStream { @@ -524,6 +524,6 @@ class Int8MinMaxCalibrator : public nvinfer1::IInt8MinMaxCalibrator // std for preprocessing std::vector m_std; }; -} // namespace tensorrt_classifier +} // namespace autoware::tensorrt_classifier -#endif // TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ +#endif // AUTOWARE__TENSORRT_CLASSIFIER__CALIBRATOR_HPP_ diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/preprocess.h similarity index 97% rename from perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h rename to perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/preprocess.h index 55295a57ad5b8..51f4e973ba7ca 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/preprocess.h +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/preprocess.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TENSORRT_CLASSIFIER__PREPROCESS_H_ -#define TENSORRT_CLASSIFIER__PREPROCESS_H_ +#ifndef AUTOWARE__TENSORRT_CLASSIFIER__PREPROCESS_H_ +#define AUTOWARE__TENSORRT_CLASSIFIER__PREPROCESS_H_ #include #include @@ -178,4 +178,4 @@ extern void multi_scale_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( float * dst, unsigned char * src, int d_w, int d_h, int d_c, Roi * d_roi, int s_w, int s_h, int s_c, int batch, float norm, cudaStream_t stream); -#endif // TENSORRT_CLASSIFIER__PREPROCESS_H_ +#endif // AUTOWARE__TENSORRT_CLASSIFIER__PREPROCESS_H_ diff --git a/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp similarity index 93% rename from perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp rename to perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp index d95ee52e8fae9..9fb1beaac39de 100644 --- a/perception/tensorrt_classifier/include/tensorrt_classifier/tensorrt_classifier.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ -#define TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ +#ifndef AUTOWARE__TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ +#define AUTOWARE__TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ #include #include @@ -25,7 +25,7 @@ #include #include -namespace tensorrt_classifier +namespace autoware::tensorrt_classifier { using cuda_utils::CudaUniquePtr; using cuda_utils::CudaUniquePtrHost; @@ -129,6 +129,6 @@ class TrtClassifier int batch_size_; CudaUniquePtrHost out_prob_h_; }; -} // namespace tensorrt_classifier +} // namespace autoware::tensorrt_classifier -#endif // TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ +#endif // AUTOWARE__TENSORRT_CLASSIFIER__TENSORRT_CLASSIFIER_HPP_ diff --git a/perception/tensorrt_classifier/package.xml b/perception/autoware_tensorrt_classifier/package.xml similarity index 91% rename from perception/tensorrt_classifier/package.xml rename to perception/autoware_tensorrt_classifier/package.xml index 439318c147e76..e41625886d8ae 100644 --- a/perception/tensorrt_classifier/package.xml +++ b/perception/autoware_tensorrt_classifier/package.xml @@ -1,11 +1,10 @@ - tensorrt_classifier + autoware_tensorrt_classifier 0.0.1 tensorrt classifier wrapper Dan Umeda - Mingyu Li Kotaro Uetake Shunsuke Miura diff --git a/perception/tensorrt_classifier/src/preprocess.cu b/perception/autoware_tensorrt_classifier/src/preprocess.cu similarity index 99% rename from perception/tensorrt_classifier/src/preprocess.cu rename to perception/autoware_tensorrt_classifier/src/preprocess.cu index fd3d758c2c280..07976281faafb 100644 --- a/perception/tensorrt_classifier/src/preprocess.cu +++ b/perception/autoware_tensorrt_classifier/src/preprocess.cu @@ -11,9 +11,9 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include -#include #include diff --git a/perception/tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp similarity index 94% rename from perception/tensorrt_classifier/src/tensorrt_classifier.cpp rename to perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp index b7c4ce99fa2e9..98b368ff373e8 100644 --- a/perception/tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include +#include #include -#include #include #include @@ -54,7 +54,7 @@ bool fileExists(const std::string & file_name, bool verbose) return true; } -std::vector loadListFromTextFile(const std::string filename) +std::vector loadListFromTextFile(const std::string & filename) { assert(fileExists(filename, true)); std::vector list; @@ -95,7 +95,7 @@ std::vector loadImageList(const std::string & filename, const std:: return fileList; } -namespace tensorrt_classifier +namespace autoware::tensorrt_classifier { TrtClassifier::TrtClassifier( const std::string & model_path, const std::string & precision, @@ -120,7 +120,8 @@ TrtClassifier::TrtClassifier( if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } - tensorrt_classifier::ImageStream stream(max_batch_size, input_dims, calibration_images); + autoware::tensorrt_classifier::ImageStream stream( + max_batch_size, input_dims, calibration_images); fs::path calibration_table{model_path}; std::string calibName = ""; std::string ext = ""; @@ -140,17 +141,17 @@ TrtClassifier::TrtClassifier( std::unique_ptr calibrator; if (build_config.calib_type_str == "Entropy") { - calibrator.reset( - new tensorrt_classifier::Int8EntropyCalibrator(stream, calibration_table, mean_, std_)); + calibrator.reset(new autoware::tensorrt_classifier::Int8EntropyCalibrator( + stream, calibration_table, mean_, std_)); } else if ( build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { double quantile = 0.999999; double cutoff = 0.999999; - calibrator.reset(new tensorrt_classifier::Int8LegacyCalibrator( + calibrator.reset(new autoware::tensorrt_classifier::Int8LegacyCalibrator( stream, calibration_table, histogram_table, mean_, std_, true, quantile, cutoff)); } else { - calibrator.reset( - new tensorrt_classifier::Int8MinMaxCalibrator(stream, calibration_table, mean_, std_)); + calibrator.reset(new autoware::tensorrt_classifier::Int8MinMaxCalibrator( + stream, calibration_table, mean_, std_)); } trt_common_ = std::make_unique( model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); @@ -384,4 +385,4 @@ bool TrtClassifier::feedforwardAndDecode( } return true; } -} // namespace tensorrt_classifier +} // namespace autoware::tensorrt_classifier diff --git a/perception/tensorrt_yolox/.gitignore b/perception/autoware_tensorrt_yolox/.gitignore similarity index 100% rename from perception/tensorrt_yolox/.gitignore rename to perception/autoware_tensorrt_yolox/.gitignore diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/autoware_tensorrt_yolox/CMakeLists.txt similarity index 96% rename from perception/tensorrt_yolox/CMakeLists.txt rename to perception/autoware_tensorrt_yolox/CMakeLists.txt index cac574aff8623..1f54326f2b33a 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/autoware_tensorrt_yolox/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.17) -project(tensorrt_yolox) +project(autoware_tensorrt_yolox) find_package(tensorrt_common) if(NOT ${tensorrt_common_FOUND}) @@ -138,7 +138,7 @@ target_compile_definitions(yolox_single_image_inference_node PRIVATE ) rclcpp_components_register_node(yolox_single_image_inference_node - PLUGIN "tensorrt_yolox::YoloXSingleImageInferenceNode" + PLUGIN "autoware::tensorrt_yolox::YoloXSingleImageInferenceNode" EXECUTABLE yolox_single_image_inference ) @@ -159,7 +159,7 @@ target_compile_definitions(${PROJECT_NAME}_node PRIVATE ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "tensorrt_yolox::TrtYoloXNode" + PLUGIN "autoware::tensorrt_yolox::TrtYoloXNode" EXECUTABLE ${PROJECT_NAME}_node_exe ) diff --git a/perception/autoware_tensorrt_yolox/README.md b/perception/autoware_tensorrt_yolox/README.md new file mode 100644 index 0000000000000..b88693f18d00f --- /dev/null +++ b/perception/autoware_tensorrt_yolox/README.md @@ -0,0 +1,161 @@ +# autoware_tensorrt_yolox + +## Purpose + +This package detects target objects e.g., cars, trucks, bicycles, and pedestrians and segment target objects such as cars, trucks, buses and pedestrian, building, vegetation, road, sidewalk on a image based on [YOLOX](https://github.com/Megvii-BaseDetection/YOLOX) model with multi-header structure. + +## Inner-workings / Algorithms + +### Cite + + + +Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Series in 2021", arXiv preprint arXiv:2107.08430, 2021 [[ref](https://arxiv.org/abs/2107.08430)] + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------- | ------------------- | --------------- | +| `in/image` | `sensor_msgs/Image` | The input image | + +### Output + +| Name | Type | Description | +| ---------------- | -------------------------------------------------- | ------------------------------------------------------------------- | +| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | +| `out/image` | `sensor_msgs/Image` | The image with 2D bounding boxes for visualization | +| `out/mask` | `sensor_msgs/Image` | The semantic segmentation mask | +| `out/color_mask` | `sensor_msgs/Image` | The colorized image of semantic segmentation mask for visualization | + +## Parameters + +{{ 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 + +The label contained in detected 2D bounding boxes (i.e., `out/objects`) will be either one of the followings: + +- CAR +- PEDESTRIAN ("PERSON" will also be categorized as "PEDESTRIAN") +- BUS +- TRUCK +- BICYCLE +- MOTORCYCLE + +If other labels (case insensitive) are contained in the file specified via the `label_file` parameter, +those are labeled as `UNKNOWN`, while detected rectangles are drawn in the visualization result (`out/image`). + +The semantic segmentation mask is a gray image whose each pixel is index of one following class: + +| index | semantic name | +| ----- | ---------------- | +| 0 | road | +| 1 | building | +| 2 | wall | +| 3 | obstacle | +| 4 | traffic_light | +| 5 | traffic_sign | +| 6 | person | +| 7 | vehicle | +| 8 | bike | +| 9 | road | +| 10 | sidewalk | +| 11 | roadPaint | +| 12 | curbstone | +| 13 | crosswalk_others | +| 14 | vegetation | +| 15 | sky | + +## Onnx model + +A sample model (named `yolox-tiny.onnx`) is downloaded by ansible script on env preparation stage, if not, please, follow [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). +To accelerate Non-maximum-suppression (NMS), which is one of the common post-process after object detection inference, +`EfficientNMS_TRT` module is attached after the ordinal YOLOX (tiny) network. +The `EfficientNMS_TRT` module contains fixed values for `score_threshold` and `nms_threshold` in it, +hence these parameters are ignored when users specify ONNX models including this module. + +This package accepts both `EfficientNMS_TRT` attached ONNXs and [models published from the official YOLOX repository](https://github.com/Megvii-BaseDetection/YOLOX/tree/main/demo/ONNXRuntime#download-onnx-models) (we referred to them as "plain" models). + +In addition to `yolox-tiny.onnx`, a custom model named `yolox-sPlus-opt-pseudoV2-T4-960x960-T4-seg16cls` is either available. +This model is multi-header structure model which is based on YOLOX-s and tuned to perform more accurate detection with almost comparable execution speed with `yolox-tiny`. +To get better results with this model, users are recommended to use some specific running arguments +such as `precision:=int8`, `calibration_algorithm:=Entropy`, `clip_value:=6.0`. +Users can refer `launch/yolox_sPlus_opt.launch.xml` to see how this model can be used. +Beside detection result, this model also output image semantic segmentation result for pointcloud filtering purpose. + +All models are automatically converted to TensorRT format. +These converted files will be saved in the same directory as specified ONNX files +with `.engine` filename extension and reused from the next run. +The conversion process may take a while (**typically 10 to 20 minutes**) and the inference process is blocked +until complete the conversion, so it will take some time until detection results are published (**even until appearing in the topic list**) on the first run + +### Package acceptable model generation + +To convert users' own model that saved in PyTorch's `pth` format into ONNX, +users can exploit the converter offered by the official repository. +For the convenience, only procedures are described below. +Please refer [the official document](https://github.com/Megvii-BaseDetection/YOLOX/tree/main/demo/ONNXRuntime#convert-your-model-to-onnx) for more detail. + +#### For plain models + +1. Install dependency + + ```shell + git clone git@github.com:Megvii-BaseDetection/YOLOX.git + cd YOLOX + python3 setup.py develop --user + ``` + +2. Convert pth into ONNX + + ```shell + python3 tools/export_onnx.py \ + --output-name YOUR_YOLOX.onnx \ + -f YOUR_YOLOX.py \ + -c YOUR_YOLOX.pth + ``` + +#### For EfficientNMS_TRT embedded models + +1. Install dependency + + ```shell + git clone git@github.com:Megvii-BaseDetection/YOLOX.git + cd YOLOX + python3 setup.py develop --user + pip3 install git+ssh://git@github.com/wep21/yolox_onnx_modifier.git --user + ``` + +2. Convert pth into ONNX + + ```shell + python3 tools/export_onnx.py \ + --output-name YOUR_YOLOX.onnx \ + -f YOUR_YOLOX.py \ + -c YOUR_YOLOX.pth + --decode_in_inference + ``` + +3. Embed `EfficientNMS_TRT` to the end of YOLOX + + ```shell + yolox_onnx_modifier YOUR_YOLOX.onnx -o YOUR_YOLOX_WITH_NMS.onnx + ``` + +## Label file + +A sample label file (named `label.txt`) and semantic segmentation color map file (name `semseg_color_map.csv`) are also downloaded automatically during env preparation process +(**NOTE:** This file is incompatible with models that output labels for the COCO dataset (e.g., models from the official YOLOX repository)). + +This file represents the correspondence between class index (integer outputted from YOLOX network) and +class label (strings making understanding easier). This package maps class IDs (incremented from 0) +with labels according to the order in this file. + +## Reference repositories + +- +- +- diff --git a/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml b/perception/autoware_tensorrt_yolox/config/yolox_s_plus_opt.param.yaml similarity index 99% rename from perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml rename to perception/autoware_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/autoware_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/autoware_tensorrt_yolox/config/yolox_tiny.param.yaml similarity index 55% rename from perception/tensorrt_yolox/config/yolox_tiny.param.yaml rename to perception/autoware_tensorrt_yolox/config/yolox_tiny.param.yaml index e45742a7afb95..e1ece63a4940f 100644 --- a/perception/tensorrt_yolox/config/yolox_tiny.param.yaml +++ b/perception/autoware_tensorrt_yolox/config/yolox_tiny.param.yaml @@ -1,9 +1,33 @@ +# cspell:ignore semseg /**: 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 + + # refine segmentation mask by overlay roi class + # disable when sematic segmentation accuracy is good enough + is_roi_overlap_segment: true + + # minimum existence_probability of detected roi considered to replace segmentation + overlap_roi_score_threshold: 0.3 + + # publish color mask for result visualization + is_publish_color_mask: false + + roi_overlay_segment_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + MOTORCYCLE : true + BICYCLE : true + PEDESTRIAN : true + ANIMAL: true + + 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. + color_map_path: "$(var data_path)/tensorrt_yolox/semseg_color_map.csv" + 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/include/tensorrt_yolox/calibrator.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/calibrator.hpp similarity index 98% rename from perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp rename to perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/calibrator.hpp index be7d0f188ff9e..73f24b644eb2c 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/calibrator.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/calibrator.hpp @@ -34,8 +34,8 @@ * DEALINGS IN THE SOFTWARE. */ -#ifndef TENSORRT_YOLOX__CALIBRATOR_HPP_ -#define TENSORRT_YOLOX__CALIBRATOR_HPP_ +#ifndef AUTOWARE__TENSORRT_YOLOX__CALIBRATOR_HPP_ +#define AUTOWARE__TENSORRT_YOLOX__CALIBRATOR_HPP_ #include "cuda_utils/cuda_check_error.hpp" #include "cuda_utils/cuda_unique_ptr.hpp" @@ -53,7 +53,7 @@ #include #include -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { class ImageStream { @@ -488,6 +488,6 @@ class Int8MinMaxCalibrator : public nvinfer1::IInt8MinMaxCalibrator std::vector hist_cache_; double scale_; }; -} // namespace tensorrt_yolox +} // namespace autoware::tensorrt_yolox -#endif // TENSORRT_YOLOX__CALIBRATOR_HPP_ +#endif // AUTOWARE__TENSORRT_YOLOX__CALIBRATOR_HPP_ diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/preprocess.hpp similarity index 96% rename from perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp rename to perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/preprocess.hpp index faac6de4e3284..947d6cc6ecc95 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/preprocess.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TENSORRT_YOLOX__PREPROCESS_HPP_ -#define TENSORRT_YOLOX__PREPROCESS_HPP_ +#ifndef AUTOWARE__TENSORRT_YOLOX__PREPROCESS_HPP_ +#define AUTOWARE__TENSORRT_YOLOX__PREPROCESS_HPP_ #include #include @@ -21,7 +21,7 @@ #include #include -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { struct Roi { @@ -195,5 +195,5 @@ extern void multi_scale_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( extern void argmax_gpu( unsigned char * dst, float * src, int d_w, int d_h, int s_w, int s_h, int s_c, int batch, cudaStream_t stream); -} // namespace tensorrt_yolox -#endif // TENSORRT_YOLOX__PREPROCESS_HPP_ +} // namespace autoware::tensorrt_yolox +#endif // AUTOWARE__TENSORRT_YOLOX__PREPROCESS_HPP_ diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp similarity index 97% rename from perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp rename to perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp index d287c8a44d4cf..6610476dc33fc 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TENSORRT_YOLOX__TENSORRT_YOLOX_HPP_ -#define TENSORRT_YOLOX__TENSORRT_YOLOX_HPP_ +#ifndef AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_HPP_ +#define AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_HPP_ +#include #include #include #include #include -#include #include #include #include -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { using cuda_utils::CudaUniquePtr; using cuda_utils::CudaUniquePtrHost; @@ -312,6 +312,6 @@ class TrtYoloX std::vector sematic_color_map_; }; -} // namespace tensorrt_yolox +} // namespace autoware::tensorrt_yolox -#endif // TENSORRT_YOLOX__TENSORRT_YOLOX_HPP_ +#endif // AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_HPP_ diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp similarity index 92% rename from perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp rename to perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp index 48ffedb98441d..7f622b2dc867e 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ -#define TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ +#ifndef AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ +#define AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ #include "object_recognition_utils/object_recognition_utils.hpp" +#include #include #include #include #include #include -#include #include #include @@ -42,7 +42,7 @@ #include #include -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { // cspell: ignore Semseg using LabelMap = std::map; @@ -110,6 +110,6 @@ class TrtYoloXNode : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace tensorrt_yolox +} // namespace autoware::tensorrt_yolox -#endif // TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ +#endif // AUTOWARE__TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ diff --git a/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml similarity index 82% rename from perception/tensorrt_yolox/launch/multiple_yolox.launch.xml rename to perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml index 9de37c76e034b..a039d25222490 100644 --- a/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml @@ -26,42 +26,42 @@ - + - + - + - + - + - + - + - + diff --git a/perception/tensorrt_yolox/launch/yolox.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox.launch.xml similarity index 100% rename from perception/tensorrt_yolox/launch/yolox.launch.xml rename to perception/autoware_tensorrt_yolox/launch/yolox.launch.xml diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml similarity index 72% rename from perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml rename to perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index e4436a0424be5..0096a219c8573 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -10,18 +10,18 @@ description="options `yolox-sPlus-T4-960x960-pseudo-finetune` if only detection is needed, `yolox-sPlus-opt-pseudoV2-T4-960x960-T4-seg16cls` if sematic segmentation is also needed" /> - + - - + + - + diff --git a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox_tiny.launch.xml similarity index 62% rename from perception/tensorrt_yolox/launch/yolox_tiny.launch.xml rename to perception/autoware_tensorrt_yolox/launch/yolox_tiny.launch.xml index 9e5d1c371b13b..a3708d14b5392 100644 --- a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/yolox_tiny.launch.xml @@ -4,16 +4,18 @@ - + - + + + - + diff --git a/perception/tensorrt_yolox/package.xml b/perception/autoware_tensorrt_yolox/package.xml similarity index 92% rename from perception/tensorrt_yolox/package.xml rename to perception/autoware_tensorrt_yolox/package.xml index 8e5a0ef973caa..9205b0c83cbea 100644 --- a/perception/tensorrt_yolox/package.xml +++ b/perception/autoware_tensorrt_yolox/package.xml @@ -1,6 +1,6 @@ - tensorrt_yolox + autoware_tensorrt_yolox 0.0.1 tensorrt library implementation for yolox @@ -30,7 +30,7 @@ tensorrt_common tier4_perception_msgs - image_transport_decompressor + autoware_image_transport_decompressor ament_lint_auto autoware_lint_common diff --git a/perception/tensorrt_yolox/schema/yolox_s_plus_opt.schema.json b/perception/autoware_tensorrt_yolox/schema/yolox_s_plus_opt.schema.json similarity index 100% rename from perception/tensorrt_yolox/schema/yolox_s_plus_opt.schema.json rename to perception/autoware_tensorrt_yolox/schema/yolox_s_plus_opt.schema.json diff --git a/perception/tensorrt_yolox/schema/yolox_tiny.schema.json b/perception/autoware_tensorrt_yolox/schema/yolox_tiny.schema.json similarity index 100% rename from perception/tensorrt_yolox/schema/yolox_tiny.schema.json rename to perception/autoware_tensorrt_yolox/schema/yolox_tiny.schema.json diff --git a/perception/tensorrt_yolox/src/preprocess.cu b/perception/autoware_tensorrt_yolox/src/preprocess.cu similarity index 99% rename from perception/tensorrt_yolox/src/preprocess.cu rename to perception/autoware_tensorrt_yolox/src/preprocess.cu index f384de2975aa4..ba9d456f32749 100644 --- a/perception/tensorrt_yolox/src/preprocess.cu +++ b/perception/autoware_tensorrt_yolox/src/preprocess.cu @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -21,7 +21,7 @@ #define MIN(x, y) x < y ? x : y -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { constexpr size_t block = 512; @@ -629,4 +629,4 @@ void argmax_gpu( N, dst, src, d_h, d_w, s_c, s_h, s_w, batch); } -} // namespace tensorrt_yolox +} // namespace autoware::tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp similarity index 98% rename from perception/tensorrt_yolox/src/tensorrt_yolox.cpp rename to perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp index 25f762e2a4ec8..1d468f7a647a5 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp @@ -15,10 +15,10 @@ #include "cuda_utils/cuda_check_error.hpp" #include "cuda_utils/cuda_unique_ptr.hpp" +#include +#include +#include #include -#include -#include -#include #include @@ -104,9 +104,9 @@ std::vector loadImageList(const std::string & filename, const std:: return fileList; } -std::vector get_seg_colormap(const std::string & filename) +std::vector get_seg_colormap(const std::string & filename) { - std::vector seg_cmap; + std::vector seg_cmap; if (filename != "not-specified") { std::vector color_list = loadListFromTextFile(filename); for (int i = 0; i < static_cast(color_list.size()); i++) { @@ -115,7 +115,7 @@ std::vector get_seg_colormap(const std::string & filen continue; } std::string colormapString = color_list[i]; - tensorrt_yolox::Colormap cmap; + autoware::tensorrt_yolox::Colormap cmap; size_t npos = colormapString.find_first_of(','); assert(npos != std::string::npos); std::string substr = colormapString.substr(0, npos); @@ -151,7 +151,7 @@ std::vector get_seg_colormap(const std::string & filen } // anonymous namespace -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { TrtYoloX::TrtYoloX( const std::string & model_path, const std::string & precision, const int num_class, @@ -1247,7 +1247,8 @@ int TrtYoloX::getMultitaskNum(void) } void TrtYoloX::getColorizedMask( - const std::vector & colormap, const cv::Mat & mask, cv::Mat & cmask) + const std::vector & colormap, const cv::Mat & mask, + cv::Mat & cmask) { int width = mask.cols; int height = mask.rows; @@ -1264,4 +1265,4 @@ void TrtYoloX::getColorizedMask( } } -} // namespace tensorrt_yolox +} // namespace autoware::tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp similarity index 75% rename from perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp rename to perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index c13384918bd9c..f3c7b2552edb6 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tensorrt_yolox/tensorrt_yolox_node.hpp" +#include "autoware/tensorrt_yolox/tensorrt_yolox_node.hpp" #include "object_recognition_utils/object_classification.hpp" @@ -24,7 +24,7 @@ #include #include -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) : Node("tensorrt_yolox", node_options) @@ -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(); @@ -320,7 +281,7 @@ void TrtYoloXNode::overlapSegmentByRoi( .rowRange(roi_y_offset, roi_y_offset + roi_height)); } -} // namespace tensorrt_yolox +} // namespace autoware::tensorrt_yolox #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(tensorrt_yolox::TrtYoloXNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::tensorrt_yolox::TrtYoloXNode) diff --git a/perception/tensorrt_yolox/src/yolox_single_image_inference_node.cpp b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp similarity index 92% rename from perception/tensorrt_yolox/src/yolox_single_image_inference_node.cpp rename to perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp index 360f41e470e38..919d68202a9a9 100644 --- a/perception/tensorrt_yolox/src/yolox_single_image_inference_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) #include @@ -26,7 +26,7 @@ namespace fs = ::std::experimental::filesystem; #include #include -namespace tensorrt_yolox +namespace autoware::tensorrt_yolox { class YoloXSingleImageInferenceNode : public rclcpp::Node { @@ -67,7 +67,7 @@ class YoloXSingleImageInferenceNode : public rclcpp::Node rclcpp::shutdown(); } }; -} // namespace tensorrt_yolox +} // namespace autoware::tensorrt_yolox #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(tensorrt_yolox::YoloXSingleImageInferenceNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::tensorrt_yolox::YoloXSingleImageInferenceNode) 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 58% 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..057c68f065c6d 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 @@ -30,8 +30,16 @@ class SSP : public GnnSolverInterface void maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) override; + std::unordered_map * reverse_assignment) override + { + const bool sparse_cost = true; + maximizeLinearAssignment(cost, direct_assignment, reverse_assignment, sparse_cost); + } + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment, const bool sparse_cost = true); }; } // 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 92% 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 e0ef193b8306b..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 @@ -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 98% 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..bc9719a5b30c6 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 @@ -45,12 +45,8 @@ struct ResidualEdge void SSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, - std::unordered_map * reverse_assignment) + std::unordered_map * reverse_assignment, const bool sparse_cost) { - // NOTE: Need to set as default arguments - bool sparse_cost = true; - // bool sparse_cost = false; - // Hyperparameters // double MAX_COST = 6; const double MAX_COST = 10; 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/traffic_light_arbiter/CMakeLists.txt b/perception/autoware_traffic_light_arbiter/CMakeLists.txt similarity index 80% rename from perception/traffic_light_arbiter/CMakeLists.txt rename to perception/autoware_traffic_light_arbiter/CMakeLists.txt index a350bdcf73cba..607f5f2e55b7a 100644 --- a/perception/traffic_light_arbiter/CMakeLists.txt +++ b/perception/autoware_traffic_light_arbiter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(traffic_light_arbiter) +project(autoware_traffic_light_arbiter) find_package(autoware_cmake REQUIRED) autoware_package() @@ -10,7 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_nodes(${PROJECT_NAME} - TrafficLightArbiter + "autoware::TrafficLightArbiter" ) ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/perception/traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md similarity index 100% rename from perception/traffic_light_arbiter/README.md rename to perception/autoware_traffic_light_arbiter/README.md diff --git a/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml similarity index 100% rename from perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml rename to perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/signal_match_validator.hpp similarity index 94% rename from perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp rename to perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/signal_match_validator.hpp index 721341bed915d..b3ede86b3a02f 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp +++ b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/signal_match_validator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ -#define TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ +#ifndef AUTOWARE__TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ +#define AUTOWARE__TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ #include #include @@ -100,4 +100,4 @@ class SignalMatchValidator bool isPedestrianSignal(const lanelet::Id & signal_id); }; -#endif // TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ +#endif // AUTOWARE__TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp similarity index 87% rename from perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp rename to perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp index e8f54970b78a2..ccd928d52b367 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ -#define TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ +#ifndef AUTOWARE__TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ +#define AUTOWARE__TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ +#include #include -#include #include #include @@ -26,6 +26,8 @@ #include #include +namespace autoware +{ class TrafficLightArbiter : public rclcpp::Node { public: @@ -58,5 +60,6 @@ class TrafficLightArbiter : public rclcpp::Node TrafficSignalArray latest_external_msg_; std::unique_ptr signal_match_validator_; }; +} // namespace autoware -#endif // TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ +#endif // AUTOWARE__TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ diff --git a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml b/perception/autoware_traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml similarity index 82% rename from perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml rename to perception/autoware_traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml index eca4b8a61b5af..8e2b9e8cf02d3 100644 --- a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml +++ b/perception/autoware_traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml @@ -2,11 +2,11 @@ - + - + diff --git a/perception/traffic_light_arbiter/package.xml b/perception/autoware_traffic_light_arbiter/package.xml similarity index 95% rename from perception/traffic_light_arbiter/package.xml rename to perception/autoware_traffic_light_arbiter/package.xml index 6e8fc61bc5306..b8c2a53604ac5 100644 --- a/perception/traffic_light_arbiter/package.xml +++ b/perception/autoware_traffic_light_arbiter/package.xml @@ -1,7 +1,7 @@ - traffic_light_arbiter + autoware_traffic_light_arbiter 0.1.0 The traffic_light_arbiter package Kenzo Lobos-Tsunekawa diff --git a/perception/traffic_light_arbiter/src/signal_match_validator.cpp b/perception/autoware_traffic_light_arbiter/src/signal_match_validator.cpp similarity index 99% rename from perception/traffic_light_arbiter/src/signal_match_validator.cpp rename to perception/autoware_traffic_light_arbiter/src/signal_match_validator.cpp index 191c07c0e3769..d964f8b2cd7ba 100644 --- a/perception/traffic_light_arbiter/src/signal_match_validator.cpp +++ b/perception/autoware_traffic_light_arbiter/src/signal_match_validator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_arbiter/signal_match_validator.hpp" +#include "autoware/traffic_light_arbiter/signal_match_validator.hpp" namespace util { diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp similarity index 97% rename from perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp rename to perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp index 033c7fbfb6a35..e71629fa5dd28 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include -#include #include #include @@ -65,6 +65,8 @@ std::vector filter_pedestrian_signals(const LaneletMapCons } // namespace lanelet +namespace autoware +{ TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) : Node("traffic_light_arbiter", options) { @@ -228,6 +230,7 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim pub_->publish(output_signals_msg); } +} // namespace autoware #include -RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightArbiter) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::TrafficLightArbiter) diff --git a/perception/traffic_light_classifier/.gitignore b/perception/autoware_traffic_light_classifier/.gitignore similarity index 100% rename from perception/traffic_light_classifier/.gitignore rename to perception/autoware_traffic_light_classifier/.gitignore diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/autoware_traffic_light_classifier/CMakeLists.txt similarity index 83% rename from perception/traffic_light_classifier/CMakeLists.txt rename to perception/autoware_traffic_light_classifier/CMakeLists.txt index a81dc6ab1e6ce..d132577743ab0 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/autoware_traffic_light_classifier/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(traffic_light_classifier) +project(autoware_traffic_light_classifier) find_package(autoware_cmake REQUIRED) autoware_package() @@ -79,9 +79,9 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_add_library(traffic_light_classifier_nodelet SHARED - src/color_classifier.cpp - src/cnn_classifier.cpp - src/nodelet.cpp + src/classifier/color_classifier.cpp + src/classifier/cnn_classifier.cpp + src/traffic_light_classifier_node.cpp ) target_link_libraries(traffic_light_classifier_nodelet ${OpenCV_LIBRARIES} @@ -94,14 +94,14 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) stdc++fs ) rclcpp_components_register_node(traffic_light_classifier_nodelet - PLUGIN "traffic_light::TrafficLightClassifierNodelet" + PLUGIN "autoware::traffic_light::TrafficLightClassifierNodelet" EXECUTABLE traffic_light_classifier_node ) ament_auto_add_library(single_image_debug_inference_node SHARED - src/cnn_classifier.cpp - src/color_classifier.cpp - src/nodelet.cpp + src/classifier/cnn_classifier.cpp + src/classifier/color_classifier.cpp + src/traffic_light_classifier_node.cpp src/single_image_debug_inference_node.cpp ) target_link_libraries(single_image_debug_inference_node @@ -117,7 +117,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) opencv_highgui ) rclcpp_components_register_node(single_image_debug_inference_node - PLUGIN "traffic_light::SingleImageDebugInferenceNode" + PLUGIN "autoware::traffic_light::SingleImageDebugInferenceNode" EXECUTABLE single_image_debug_inference ) @@ -133,16 +133,16 @@ else() ${OpenCV_INCLUDE_DIRS} ) - ament_auto_add_library(traffic_light_classifier_nodelet SHARED - src/color_classifier.cpp - src/nodelet.cpp + ament_auto_add_library(${PROJECT_NAME} SHARED + src/classifier/color_classifier.cpp + src/traffic_light_classifier_node.cpp ) - target_link_libraries(traffic_light_classifier_nodelet + target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ) - rclcpp_components_register_node(traffic_light_classifier_nodelet - PLUGIN "traffic_light::TrafficLightClassifierNodelet" + rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::TrafficLightClassifierNodelet" EXECUTABLE traffic_light_classifier_node ) diff --git a/perception/traffic_light_classifier/README.md b/perception/autoware_traffic_light_classifier/README.md similarity index 100% rename from perception/traffic_light_classifier/README.md rename to perception/autoware_traffic_light_classifier/README.md diff --git a/perception/traffic_light_classifier/config/car_traffic_light_classifier.param.yaml b/perception/autoware_traffic_light_classifier/config/car_traffic_light_classifier.param.yaml similarity index 100% rename from perception/traffic_light_classifier/config/car_traffic_light_classifier.param.yaml rename to perception/autoware_traffic_light_classifier/config/car_traffic_light_classifier.param.yaml diff --git a/perception/traffic_light_classifier/config/pedestrian_traffic_light_classifier.param.yaml b/perception/autoware_traffic_light_classifier/config/pedestrian_traffic_light_classifier.param.yaml similarity index 100% rename from perception/traffic_light_classifier/config/pedestrian_traffic_light_classifier.param.yaml rename to perception/autoware_traffic_light_classifier/config/pedestrian_traffic_light_classifier.param.yaml diff --git a/perception/traffic_light_classifier/image/TrafficLightDataStructure.jpg b/perception/autoware_traffic_light_classifier/image/TrafficLightDataStructure.jpg similarity index 100% rename from perception/traffic_light_classifier/image/TrafficLightDataStructure.jpg rename to perception/autoware_traffic_light_classifier/image/TrafficLightDataStructure.jpg diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/autoware_traffic_light_classifier/launch/traffic_light_classifier.launch.xml similarity index 72% rename from perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml rename to perception/autoware_traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 8348513e23c53..d0cbbd3dcae9b 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/autoware_traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -2,10 +2,10 @@ - + - + diff --git a/perception/traffic_light_classifier/package.xml b/perception/autoware_traffic_light_classifier/package.xml similarity index 85% rename from perception/traffic_light_classifier/package.xml rename to perception/autoware_traffic_light_classifier/package.xml index 9e460e8ad1a1d..6da678e02cfb2 100644 --- a/perception/traffic_light_classifier/package.xml +++ b/perception/autoware_traffic_light_classifier/package.xml @@ -1,9 +1,9 @@ - traffic_light_classifier + autoware_traffic_light_classifier 0.1.0 - The traffic_light_classifier package + The autoware_traffic_light_classifier package Yukihiro Saito Shunsuke Miura Tao Zhong @@ -14,6 +14,7 @@ autoware_cmake + autoware_tensorrt_classifier cuda_utils cv_bridge image_transport @@ -22,7 +23,6 @@ rclcpp rclcpp_components sensor_msgs - tensorrt_classifier tensorrt_common tier4_perception_msgs diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp b/perception/autoware_traffic_light_classifier/src/classifier/classifier_interface.hpp similarity index 79% rename from perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp rename to perception/autoware_traffic_light_classifier/src/classifier/classifier_interface.hpp index 563850da9d2f4..cff5828a5b00f 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/classifier_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ +#ifndef CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ +#define CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ #include #include @@ -22,7 +22,7 @@ #include -namespace traffic_light +namespace autoware::traffic_light { class ClassifierInterface { @@ -31,6 +31,6 @@ class ClassifierInterface const std::vector & input_image, tier4_perception_msgs::msg::TrafficLightArray & traffic_signals) = 0; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ +#endif // CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ diff --git a/perception/traffic_light_classifier/src/cnn_classifier.cpp b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp similarity index 97% rename from perception/traffic_light_classifier/src/cnn_classifier.cpp rename to perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp index d6ba81041c6f8..6e3ae34297510 100644 --- a/perception/traffic_light_classifier/src/cnn_classifier.cpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_classifier/cnn_classifier.hpp" +#include "cnn_classifier.hpp" #include #include #include +#include #include #include #include -namespace traffic_light +namespace autoware::traffic_light { CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) { @@ -55,7 +56,7 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) batch_size_ = input_dim.d[0]; tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; - classifier_ = std::make_unique( + classifier_ = std::make_unique( model_file_path, precision, batch_config, mean_, std_); if (node_ptr_->declare_parameter("build_only", false)) { RCLCPP_INFO(node_ptr_->get_logger(), "TensorRT engine is built and shutdown node."); @@ -207,4 +208,4 @@ bool CNNClassifier::isColorLabel(const std::string label) return false; } -} // namespace traffic_light +} // namespace autoware::traffic_light diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.hpp similarity index 90% rename from perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp rename to perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.hpp index 545a080763ef4..5f6f086763f4e 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.hpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ +#ifndef CLASSIFIER__CNN_CLASSIFIER_HPP_ +#define CLASSIFIER__CNN_CLASSIFIER_HPP_ -#include "traffic_light_classifier/classifier_interface.hpp" +#include "classifier_interface.hpp" +#include #include #include #include @@ -24,7 +25,6 @@ #include #include #include -#include #include #include @@ -41,7 +41,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { using cuda_utils::CudaUniquePtr; @@ -60,7 +60,8 @@ class CNNClassifier : public ClassifierInterface tier4_perception_msgs::msg::TrafficLightArray & traffic_signals) override; private: - void postProcess(int cls, float prob, tier4_perception_msgs::msg::TrafficLight & traffic_signal); + void postProcess( + int class_index, float prob, tier4_perception_msgs::msg::TrafficLight & traffic_signal); bool readLabelfile(std::string filepath, std::vector & labels); bool isColorLabel(const std::string label); void outputDebugImage( @@ -111,13 +112,13 @@ class CNNClassifier : public ClassifierInterface rclcpp::Node * node_ptr_; int batch_size_; - std::unique_ptr classifier_; + std::unique_ptr classifier_; image_transport::Publisher image_pub_; std::vector labels_; std::vector mean_; std::vector std_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ +#endif // CLASSIFIER__CNN_CLASSIFIER_HPP_ diff --git a/perception/traffic_light_classifier/src/color_classifier.cpp b/perception/autoware_traffic_light_classifier/src/classifier/color_classifier.cpp similarity index 99% rename from perception/traffic_light_classifier/src/color_classifier.cpp rename to perception/autoware_traffic_light_classifier/src/classifier/color_classifier.cpp index 604568b585a63..6f9800683aa27 100644 --- a/perception/traffic_light_classifier/src/color_classifier.cpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/color_classifier.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_classifier/color_classifier.hpp" +#include "color_classifier.hpp" #include @@ -19,7 +19,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { ColorClassifier::ColorClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) { @@ -255,4 +255,4 @@ rcl_interfaces::msg::SetParametersResult ColorClassifier::parametersCallback( return result; } -} // namespace traffic_light +} // namespace autoware::traffic_light diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp b/perception/autoware_traffic_light_classifier/src/classifier/color_classifier.hpp similarity index 88% rename from perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp rename to perception/autoware_traffic_light_classifier/src/classifier/color_classifier.hpp index 38f2d7f6a48db..6e11a6f6f3d84 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/color_classifier.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ +#ifndef CLASSIFIER__COLOR_CLASSIFIER_HPP_ +#define CLASSIFIER__COLOR_CLASSIFIER_HPP_ -#include "traffic_light_classifier/classifier_interface.hpp" +#include "classifier_interface.hpp" #include #include @@ -32,7 +32,7 @@ #include -namespace traffic_light +namespace autoware::traffic_light { struct HSVConfig { @@ -93,6 +93,6 @@ class ColorClassifier : public ClassifierInterface cv::Scalar max_hsv_red_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ +#endif // CLASSIFIER__COLOR_CLASSIFIER_HPP_ diff --git a/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp b/perception/autoware_traffic_light_classifier/src/single_image_debug_inference_node.cpp similarity index 95% rename from perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp rename to perception/autoware_traffic_light_classifier/src/single_image_debug_inference_node.cpp index f6f963086bdd7..245dc069af163 100644 --- a/perception/traffic_light_classifier/src/single_image_debug_inference_node.cpp +++ b/perception/autoware_traffic_light_classifier/src/single_image_debug_inference_node.cpp @@ -15,11 +15,11 @@ #include #if ENABLE_GPU -#include +#include "classifier/cnn_classifier.hpp" #endif -#include -#include +#include "classifier/color_classifier.hpp" +#include "traffic_light_classifier_node.hpp" #include #include @@ -60,7 +60,7 @@ std::string toString(const uint8_t state) } } // namespace -namespace traffic_light +namespace autoware::traffic_light { class SingleImageDebugInferenceNode : public rclcpp::Node { @@ -159,7 +159,7 @@ class SingleImageDebugInferenceNode : public rclcpp::Node cv::Mat image_; std::unique_ptr classifier_ptr_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::SingleImageDebugInferenceNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::SingleImageDebugInferenceNode) diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp similarity index 96% rename from perception/traffic_light_classifier/src/nodelet.cpp rename to perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp index 6378a98fdc8cc..b42ded4cbffa3 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_classifier/nodelet.hpp" +#include "traffic_light_classifier_node.hpp" #include @@ -20,7 +20,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeOptions & options) : Node("traffic_light_classifier_node", options) @@ -169,8 +169,8 @@ bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) cons return backlight_threshold_ <= intensity; } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightClassifierNodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightClassifierNodelet) diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.hpp similarity index 88% rename from perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp rename to perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.hpp index d48dd8aba3b95..d86880744929f 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ -#define TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ +#ifndef TRAFFIC_LIGHT_CLASSIFIER_NODE_HPP_ +#define TRAFFIC_LIGHT_CLASSIFIER_NODE_HPP_ -#include "traffic_light_classifier/classifier_interface.hpp" +#include "classifier/classifier_interface.hpp" #include #include @@ -43,15 +43,15 @@ #include #if ENABLE_GPU -#include "traffic_light_classifier/cnn_classifier.hpp" +#include "classifier/cnn_classifier.hpp" #endif -#include "traffic_light_classifier/color_classifier.hpp" +#include "classifier/color_classifier.hpp" #include #include -namespace traffic_light +namespace autoware::traffic_light { class TrafficLightClassifierNodelet : public rclcpp::Node { @@ -93,6 +93,6 @@ class TrafficLightClassifierNodelet : public rclcpp::Node bool is_harsh_backlight(const cv::Mat & img) const; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ +#endif // TRAFFIC_LIGHT_CLASSIFIER_NODE_HPP_ diff --git a/perception/traffic_light_fine_detector/.gitignore b/perception/autoware_traffic_light_fine_detector/.gitignore similarity index 100% rename from perception/traffic_light_fine_detector/.gitignore rename to perception/autoware_traffic_light_fine_detector/.gitignore diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt similarity index 87% rename from perception/traffic_light_fine_detector/CMakeLists.txt rename to perception/autoware_traffic_light_fine_detector/CMakeLists.txt index fe9fa2ffcaa8b..80d6e43c098ed 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(traffic_light_fine_detector) +project(autoware_traffic_light_fine_detector) find_package(autoware_cmake REQUIRED) autoware_package() @@ -75,15 +75,15 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${CUDA_INCLUDE_DIRS} ) - ament_auto_add_library(traffic_light_fine_detector_nodelet SHARED - src/nodelet.cpp + ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_fine_detector_node.cpp ) - target_include_directories(traffic_light_fine_detector_nodelet PUBLIC + target_include_directories(${PROJECT_NAME} PUBLIC lib/include ) - target_link_libraries(traffic_light_fine_detector_nodelet + target_link_libraries(${PROJECT_NAME} ${NVINFER} ${NVONNXPARSER} ${NVINFER_PLUGIN} @@ -94,8 +94,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) stdc++fs ) - rclcpp_components_register_node(traffic_light_fine_detector_nodelet - PLUGIN "traffic_light::TrafficLightFineDetectorNodelet" + rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::TrafficLightFineDetectorNode" EXECUTABLE traffic_light_fine_detector_node ) diff --git a/perception/traffic_light_fine_detector/README.md b/perception/autoware_traffic_light_fine_detector/README.md similarity index 100% rename from perception/traffic_light_fine_detector/README.md rename to perception/autoware_traffic_light_fine_detector/README.md diff --git a/perception/traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml b/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml similarity index 100% rename from perception/traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml rename to perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/autoware_traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml similarity index 66% rename from perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml rename to perception/autoware_traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 85bb79789af17..3a44e0c3bdfe0 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/autoware_traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -3,10 +3,14 @@ - + - + diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/autoware_traffic_light_fine_detector/package.xml similarity index 84% rename from perception/traffic_light_fine_detector/package.xml rename to perception/autoware_traffic_light_fine_detector/package.xml index cf2495228b0c5..38ebd7defb7d0 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/autoware_traffic_light_fine_detector/package.xml @@ -1,9 +1,9 @@ - traffic_light_fine_detector + autoware_traffic_light_fine_detector 0.1.0 - The traffic_light_fine_detector package + The autoware_traffic_light_fine_detector package Tao Zhong Shunsuke Miura Shintaro Tomie @@ -13,13 +13,13 @@ autoware_cmake + autoware_tensorrt_yolox cv_bridge image_transport message_filters rclcpp rclcpp_components sensor_msgs - tensorrt_yolox tier4_debug_msgs tier4_perception_msgs diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp similarity index 84% rename from perception/traffic_light_fine_detector/src/nodelet.cpp rename to perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index 8037dc5472fbe..381fa79ed090c 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_fine_detector/nodelet.hpp" +#include "traffic_light_fine_detector_node.hpp" #if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) #include @@ -22,6 +22,7 @@ namespace fs = ::std::filesystem; namespace fs = ::std::experimental::filesystem; #endif +#include #include #include #include @@ -30,7 +31,7 @@ namespace fs = ::std::experimental::filesystem; namespace { float calWeightedIou( - const sensor_msgs::msg::RegionOfInterest & bbox1, const tensorrt_yolox::Object & bbox2) + const sensor_msgs::msg::RegionOfInterest & bbox1, const autoware::tensorrt_yolox::Object & bbox2) { int x1 = std::max(static_cast(bbox1.x_offset), bbox2.x_offset); int x2 = std::min(static_cast(bbox1.x_offset + bbox1.width), bbox2.x_offset + bbox2.width); @@ -46,15 +47,14 @@ float calWeightedIou( } // namespace -namespace traffic_light +namespace autoware::traffic_light { -inline std::vector toFloatVector(const std::vector double_vector) +inline std::vector toFloatVector(const std::vector & double_vector) { return std::vector(double_vector.begin(), double_vector.end()); } -TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet( - const rclcpp::NodeOptions & options) +TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOptions & options) : Node("traffic_light_fine_detector_node", options) { int num_class = 2; @@ -89,13 +89,13 @@ TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet( batch_size_ = input_dim.d[0]; const tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; - trt_yolox_ = std::make_unique( + trt_yolox_ = std::make_unique( model_path, precision, num_class, score_thresh_, nms_threshold, build_config, cuda_preprocess, calib_image_list, scale, cache_dir, batch_config); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&TrafficLightFineDetectorNodelet::connectCb, this)); + this, get_clock(), 100ms, std::bind(&TrafficLightFineDetectorNode::connectCb, this)); std::lock_guard lock(connect_mutex_); output_roi_pub_ = this->create_publisher("~/output/rois", 1); @@ -105,11 +105,10 @@ TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet( approximate_sync_.reset( new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, rough_roi_sub_, expect_roi_sub_)); approximate_sync_->registerCallback( - std::bind(&TrafficLightFineDetectorNodelet::callback, this, _1, _2, _3)); + std::bind(&TrafficLightFineDetectorNode::callback, this, _1, _2, _3)); } else { sync_.reset(new Sync(SyncPolicy(10), image_sub_, rough_roi_sub_, expect_roi_sub_)); - sync_->registerCallback( - std::bind(&TrafficLightFineDetectorNodelet::callback, this, _1, _2, _3)); + sync_->registerCallback(std::bind(&TrafficLightFineDetectorNode::callback, this, _1, _2, _3)); } if (declare_parameter("build_only", false)) { @@ -118,7 +117,7 @@ TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet( } } -void TrafficLightFineDetectorNodelet::connectCb() +void TrafficLightFineDetectorNode::connectCb() { std::lock_guard lock(connect_mutex_); if (output_roi_pub_->get_subscription_count() == 0) { @@ -132,7 +131,7 @@ void TrafficLightFineDetectorNodelet::connectCb() } } -void TrafficLightFineDetectorNodelet::callback( +void TrafficLightFineDetectorNode::callback( const sensor_msgs::msg::Image::ConstSharedPtr in_image_msg, const TrafficLightRoiArray::ConstSharedPtr rough_roi_msg, const TrafficLightRoiArray::ConstSharedPtr expect_roi_msg) @@ -147,14 +146,14 @@ void TrafficLightFineDetectorNodelet::callback( cv::Mat original_image; TrafficLightRoiArray out_rois; std::map id2expectRoi; - std::map id2detections; + std::map id2detections; for (const auto & expect_roi : expect_roi_msg->rois) { id2expectRoi[expect_roi.traffic_light_id] = expect_roi; } rosMsg2CvMat(in_image_msg, original_image, "bgr8"); std::vector rois; - tensorrt_yolox::ObjectArrays inference_results; + autoware::tensorrt_yolox::ObjectArrays inference_results; std::vector lts; std::vector roi_ids; @@ -178,7 +177,7 @@ void TrafficLightFineDetectorNodelet::callback( if (static_cast(rois.size()) == batch_size_) { trt_yolox_->doMultiScaleInference(original_image, inference_results, rois); for (size_t batch_i = 0; batch_i < true_batch_size; batch_i++) { - for (const tensorrt_yolox::Object & detection : inference_results[batch_i]) { + for (const autoware::tensorrt_yolox::Object & detection : inference_results[batch_i]) { if (detection.score < score_thresh_) { continue; } @@ -186,7 +185,7 @@ void TrafficLightFineDetectorNodelet::callback( lts[batch_i].x + detection.x_offset, lts[batch_i].y + detection.y_offset); cv::Point rb_roi(lt_roi.x + detection.width, lt_roi.y + detection.height); fitInFrame(lt_roi, rb_roi, cv::Size(original_image.size())); - tensorrt_yolox::Object det = detection; + autoware::tensorrt_yolox::Object det = detection; det.x_offset = lt_roi.x; det.y_offset = lt_roi.y; det.width = rb_roi.x - lt_roi.x; @@ -213,10 +212,10 @@ void TrafficLightFineDetectorNodelet::callback( exe_time_pub_->publish(exe_time_msg); } -float TrafficLightFineDetectorNodelet::evalMatchScore( - std::map & id2expectRoi, - std::map & id2detections, - std::map & id2bestDetection) +float TrafficLightFineDetectorNode::evalMatchScore( + const std::map & id2expectRoi, + std::map & id2detections, + std::map & id2bestDetection) { float score_sum = 0.0f; id2bestDetection.clear(); @@ -224,7 +223,7 @@ float TrafficLightFineDetectorNodelet::evalMatchScore( int tlr_id = roi_p.first; float max_score = 0.0f; const sensor_msgs::msg::RegionOfInterest & expected_roi = roi_p.second.roi; - for (const tensorrt_yolox::Object & detection : id2detections[tlr_id]) { + for (const autoware::tensorrt_yolox::Object & detection : id2detections[tlr_id]) { float score = ::calWeightedIou(expected_roi, detection); if (score >= max_score) { max_score = score; @@ -236,19 +235,20 @@ float TrafficLightFineDetectorNodelet::evalMatchScore( return score_sum; } -void TrafficLightFineDetectorNodelet::detectionMatch( - std::map & id2expectRoi, - std::map & id2detections, TrafficLightRoiArray & out_rois) +void TrafficLightFineDetectorNode::detectionMatch( + const std::map & id2expectRoi, + std::map & id2detections, + TrafficLightRoiArray & out_rois) { float max_score = 0.0f; - std::map bestDetections; + std::map bestDetections; for (const auto & roi_pair : id2expectRoi) { int tlr_id = roi_pair.first; // the expected roi calculated from tf information const sensor_msgs::msg::RegionOfInterest & expect_roi = roi_pair.second.roi; int expect_cx = expect_roi.x_offset + expect_roi.width / 2; int expect_cy = expect_roi.y_offset + expect_roi.height / 2; - for (const tensorrt_yolox::Object & det : id2detections[tlr_id]) { + for (const autoware::tensorrt_yolox::Object & det : id2detections[tlr_id]) { // for every detection, calculate the center offset between the detection and the // corresponding expected roi int det_cx = det.x_offset + det.width / 2; @@ -262,7 +262,7 @@ void TrafficLightFineDetectorNodelet::detectionMatch( p.second.roi.y_offset += dy; } // calculate the "match score" between expected rois and id2detections_copy - std::map id2bestDetection; + std::map id2bestDetection; float score = evalMatchScore(id2expectRoi_copy, id2detections, id2bestDetection); if (score > max_score) { max_score = score; @@ -293,12 +293,12 @@ void TrafficLightFineDetectorNodelet::detectionMatch( for (const auto & id : invalid_roi_id) { TrafficLightRoi tlr; tlr.traffic_light_id = id; - tlr.traffic_light_type = id2expectRoi[id].traffic_light_type; + tlr.traffic_light_type = id2expectRoi.at(id).traffic_light_type; out_rois.rois.push_back(tlr); } } -bool TrafficLightFineDetectorNodelet::rosMsg2CvMat( +bool TrafficLightFineDetectorNode::rosMsg2CvMat( const sensor_msgs::msg::Image::ConstSharedPtr image_msg, cv::Mat & image, std::string encode) { try { @@ -313,8 +313,7 @@ bool TrafficLightFineDetectorNodelet::rosMsg2CvMat( return true; } -bool TrafficLightFineDetectorNodelet::fitInFrame( - cv::Point & lt, cv::Point & rb, const cv::Size & size) +bool TrafficLightFineDetectorNode::fitInFrame(cv::Point & lt, cv::Point & rb, const cv::Size & size) { const int width = static_cast(size.width); const int height = static_cast(size.height); @@ -334,7 +333,7 @@ bool TrafficLightFineDetectorNodelet::fitInFrame( return true; } -bool TrafficLightFineDetectorNodelet::readLabelFile( +bool TrafficLightFineDetectorNode::readLabelFile( const std::string & filepath, std::vector & tlr_label_id_, int & num_class) { std::ifstream labelsFile(filepath); @@ -354,7 +353,7 @@ bool TrafficLightFineDetectorNodelet::readLabelFile( return tlr_label_id_.size() != 0; } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightFineDetectorNodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightFineDetectorNode) diff --git a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.hpp similarity index 86% rename from perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp rename to perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.hpp index 5c89c76a11833..ada6be3844d77 100644 --- a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_FINE_DETECTOR__NODELET_HPP_ -#define TRAFFIC_LIGHT_FINE_DETECTOR__NODELET_HPP_ +#ifndef TRAFFIC_LIGHT_FINE_DETECTOR_NODE_HPP_ +#define TRAFFIC_LIGHT_FINE_DETECTOR_NODE_HPP_ +#include #include #include #include #include #include -#include #include #include @@ -50,15 +50,15 @@ typedef struct Detection float x, y, w, h, prob; } Detection; -namespace traffic_light +namespace autoware::traffic_light { -class TrafficLightFineDetectorNodelet : public rclcpp::Node +class TrafficLightFineDetectorNode : public rclcpp::Node { using TrafficLightRoi = tier4_perception_msgs::msg::TrafficLightRoi; using TrafficLightRoiArray = tier4_perception_msgs::msg::TrafficLightRoiArray; public: - explicit TrafficLightFineDetectorNodelet(const rclcpp::NodeOptions & options); + explicit TrafficLightFineDetectorNode(const rclcpp::NodeOptions & options); void connectCb(); /** * @brief main process function. @@ -68,7 +68,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node * @param expect_roi_msg expect rois message */ void callback( - const sensor_msgs::msg::Image::ConstSharedPtr image_msg, + const sensor_msgs::msg::Image::ConstSharedPtr in_image_msg, const TrafficLightRoiArray::ConstSharedPtr rough_roi_msg, const TrafficLightRoiArray::ConstSharedPtr expect_roi_msg); @@ -93,9 +93,9 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node * @return float */ float evalMatchScore( - std::map & id2expectRoi, - std::map & id2detections, - std::map & id2bestDetection); + const std::map & id2expectRoi, + std::map & id2detections, + std::map & id2bestDetection); /** * @brief Every traffic light roi might have several possible detections. This function * is designed to select the best detection for every traffic light by making use of @@ -113,8 +113,9 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node * @param out_rois output rois converted from the selected detections */ void detectionMatch( - std::map & id2expectRoi, - std::map & id2detections, TrafficLightRoiArray & out_rois); + const std::map & id2expectRoi, + std::map & id2detections, + TrafficLightRoiArray & out_rois); /** * @brief convert image message to cv::Mat @@ -167,9 +168,9 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node std::vector tlr_label_id_; int batch_size_; - std::unique_ptr trt_yolox_; + std::unique_ptr trt_yolox_; }; // TrafficLightFineDetectorNodelet -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_FINE_DETECTOR__NODELET_HPP_ +#endif // TRAFFIC_LIGHT_FINE_DETECTOR_NODE_HPP_ diff --git a/perception/traffic_light_map_based_detector/CMakeLists.txt b/perception/autoware_traffic_light_map_based_detector/CMakeLists.txt similarity index 54% rename from perception/traffic_light_map_based_detector/CMakeLists.txt rename to perception/autoware_traffic_light_map_based_detector/CMakeLists.txt index 75a4bf9064782..78c10447c9b0a 100644 --- a/perception/traffic_light_map_based_detector/CMakeLists.txt +++ b/perception/autoware_traffic_light_map_based_detector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(traffic_light_map_based_detector) +project(autoware_traffic_light_map_based_detector) find_package(autoware_cmake REQUIRED) autoware_package() @@ -11,12 +11,12 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(traffic_light_map_based_detector SHARED - src/node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_map_based_detector_node.cpp ) -rclcpp_components_register_node(traffic_light_map_based_detector - PLUGIN "traffic_light::MapBasedDetector" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::MapBasedDetector" EXECUTABLE traffic_light_map_based_detector_node ) diff --git a/perception/traffic_light_map_based_detector/README.md b/perception/autoware_traffic_light_map_based_detector/README.md similarity index 94% rename from perception/traffic_light_map_based_detector/README.md rename to perception/autoware_traffic_light_map_based_detector/README.md index 5f1cc3a1d44f6..8a59db19ae64d 100644 --- a/perception/traffic_light_map_based_detector/README.md +++ b/perception/autoware_traffic_light_map_based_detector/README.md @@ -1,8 +1,8 @@ -# The `traffic_light_map_based_detector` Package +# The `autoware_traffic_light_map_based_detector` Package ## Overview -`traffic_light_map_based_detector` calculates where the traffic lights will appear in the image based on the HD map. +`autoware_traffic_light_map_based_detector` calculates where the traffic lights will appear in the image based on the HD map. Calibration and vibration errors can be entered as parameters, and the size of the detected RegionOfInterest will change according to the error. diff --git a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml b/perception/autoware_traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml similarity index 100% rename from perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml rename to perception/autoware_traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml diff --git a/perception/traffic_light_map_based_detector/docs/traffic_light_map_based_detector_result.svg b/perception/autoware_traffic_light_map_based_detector/docs/traffic_light_map_based_detector_result.svg similarity index 100% rename from perception/traffic_light_map_based_detector/docs/traffic_light_map_based_detector_result.svg rename to perception/autoware_traffic_light_map_based_detector/docs/traffic_light_map_based_detector_result.svg diff --git a/perception/traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml b/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml similarity index 79% rename from perception/traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml rename to perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml index 3b7d7309e4325..9f035e7f1ae6c 100644 --- a/perception/traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml +++ b/perception/autoware_traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml @@ -10,9 +10,9 @@ - + - + diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/autoware_traffic_light_map_based_detector/package.xml similarity index 87% rename from perception/traffic_light_map_based_detector/package.xml rename to perception/autoware_traffic_light_map_based_detector/package.xml index 7553b136d791a..8222c48f989c7 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/autoware_traffic_light_map_based_detector/package.xml @@ -1,9 +1,9 @@ - traffic_light_map_based_detector + autoware_traffic_light_map_based_detector 0.1.0 - The traffic_light_map_based_detector package + The autoware_traffic_light_map_based_detector package Yukihiro Saito Shunsuke Miura Tao Zhong @@ -15,7 +15,6 @@ autoware_lanelet2_extension autoware_map_msgs - autoware_planning_msgs autoware_universe_utils geometry_msgs image_geometry diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp similarity index 98% rename from perception/traffic_light_map_based_detector/src/node.cpp rename to perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp index 7d241d71ce185..d133fe0b443d1 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.cpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_map_based_detector/node.hpp" +#define EIGEN_MPL2_ONLY + +#include "traffic_light_map_based_detector_node.hpp" +#include +#include #include #include #include @@ -27,10 +31,6 @@ #include #include -#define EIGEN_MPL2_ONLY -#include -#include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -118,7 +118,7 @@ tf2::Vector3 getTrafficLightCenter(const lanelet::ConstLineString3d & traffic_li } // namespace -namespace traffic_light +namespace autoware::traffic_light { MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options) : Node("traffic_light_map_based_detector", node_options), @@ -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; @@ -605,7 +605,7 @@ void MapBasedDetector::publishVisibleTrafficLights( } pub->publish(output_msg); } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::MapBasedDetector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::MapBasedDetector) diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.hpp similarity index 96% rename from perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp rename to perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.hpp index 3aeba4c4a913a..de4d464932698 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/autoware_traffic_light_map_based_detector/src/traffic_light_map_based_detector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ -#define TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_MAP_BASED_DETECTOR_NODE_HPP_ +#define TRAFFIC_LIGHT_MAP_BASED_DETECTOR_NODE_HPP_ #include #include @@ -44,7 +44,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { class MapBasedDetector : public rclcpp::Node { @@ -182,7 +182,7 @@ class MapBasedDetector : public rclcpp::Node const std::vector & tf_map2camera_vec, const image_geometry::PinholeCameraModel & pinhole_camera_model, const lanelet::ConstLineString3d traffic_light, const Config & config, - tier4_perception_msgs::msg::TrafficLightRoi & roi) const; + tier4_perception_msgs::msg::TrafficLightRoi & out_roi) const; /** * @brief Publish the traffic lights for visualization * @@ -196,5 +196,5 @@ class MapBasedDetector : public rclcpp::Node const std::vector & visible_traffic_lights, const rclcpp::Publisher::SharedPtr pub); }; -} // namespace traffic_light -#endif // TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ +} // namespace autoware::traffic_light +#endif // TRAFFIC_LIGHT_MAP_BASED_DETECTOR_NODE_HPP_ diff --git a/perception/autoware_traffic_light_multi_camera_fusion/CMakeLists.txt b/perception/autoware_traffic_light_multi_camera_fusion/CMakeLists.txt new file mode 100644 index 0000000000000..916b440d5b322 --- /dev/null +++ b/perception/autoware_traffic_light_multi_camera_fusion/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_traffic_light_multi_camera_fusion) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +include_directories( + SYSTEM +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_multi_camera_fusion_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::traffic_light::MultiCameraFusion" + EXECUTABLE traffic_light_multi_camera_fusion_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/perception/traffic_light_multi_camera_fusion/README.md b/perception/autoware_traffic_light_multi_camera_fusion/README.md similarity index 100% rename from perception/traffic_light_multi_camera_fusion/README.md rename to perception/autoware_traffic_light_multi_camera_fusion/README.md diff --git a/perception/traffic_light_multi_camera_fusion/config/traffic_light_multi_camera_fusion.param.yaml b/perception/autoware_traffic_light_multi_camera_fusion/config/traffic_light_multi_camera_fusion.param.yaml similarity index 100% rename from perception/traffic_light_multi_camera_fusion/config/traffic_light_multi_camera_fusion.param.yaml rename to perception/autoware_traffic_light_multi_camera_fusion/config/traffic_light_multi_camera_fusion.param.yaml diff --git a/perception/traffic_light_multi_camera_fusion/launch/traffic_light_multi_camera_fusion.launch.xml b/perception/autoware_traffic_light_multi_camera_fusion/launch/traffic_light_multi_camera_fusion.launch.xml similarity index 56% rename from perception/traffic_light_multi_camera_fusion/launch/traffic_light_multi_camera_fusion.launch.xml rename to perception/autoware_traffic_light_multi_camera_fusion/launch/traffic_light_multi_camera_fusion.launch.xml index 9aa4958378c1c..32e3417cf9029 100644 --- a/perception/traffic_light_multi_camera_fusion/launch/traffic_light_multi_camera_fusion.launch.xml +++ b/perception/autoware_traffic_light_multi_camera_fusion/launch/traffic_light_multi_camera_fusion.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/perception/traffic_light_multi_camera_fusion/package.xml b/perception/autoware_traffic_light_multi_camera_fusion/package.xml similarity index 86% rename from perception/traffic_light_multi_camera_fusion/package.xml rename to perception/autoware_traffic_light_multi_camera_fusion/package.xml index db942d4bb60b9..4e510d6478f3e 100644 --- a/perception/traffic_light_multi_camera_fusion/package.xml +++ b/perception/autoware_traffic_light_multi_camera_fusion/package.xml @@ -1,9 +1,9 @@ - traffic_light_multi_camera_fusion + autoware_traffic_light_multi_camera_fusion 0.1.0 - The traffic_light_multi_camera_fusion package + The autoware_traffic_light_multi_camera_fusion package Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp similarity index 96% rename from perception/traffic_light_multi_camera_fusion/src/node.cpp rename to perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp index 607aabdefa3cc..70841b936af37 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_multi_camera_fusion/node.hpp" +#include "traffic_light_multi_camera_fusion_node.hpp" #include #include @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace @@ -39,7 +40,7 @@ bool isUnknown(const tier4_perception_msgs::msg::TrafficLight & signal) * @param record fusion record * @return 0 if traffic light is truncated, otherwise 1 */ -int calVisibleScore(const traffic_light::FusionRecord & record) +int calVisibleScore(const autoware::traffic_light::FusionRecord & record) { const uint32_t boundary = 5; uint32_t x1 = record.roi.roi.x_offset; @@ -55,7 +56,9 @@ int calVisibleScore(const traffic_light::FusionRecord & record) } } -int compareRecord(const traffic_light::FusionRecord & r1, const traffic_light::FusionRecord & r2) +int compareRecord( + const autoware::traffic_light::FusionRecord & r1, + const autoware::traffic_light::FusionRecord & r2) { /* if both records are from the same sensor but different stamp, trust the latest one @@ -133,7 +136,7 @@ autoware_perception_msgs::msg::TrafficLightElement convert( } // namespace -namespace traffic_light +namespace autoware::traffic_light { MultiCameraFusion::MultiCameraFusion(const rclcpp::NodeOptions & node_options) @@ -293,7 +296,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 @@ -319,7 +322,7 @@ void MultiCameraFusion::groupFusion( } } -} // namespace traffic_light +} // namespace autoware::traffic_light #include -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::MultiCameraFusion) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::MultiCameraFusion) diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.hpp similarity index 95% rename from perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp rename to perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.hpp index 4389b5d3c1aa1..23bc59c26b293 100644 --- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp +++ b/perception/autoware_traffic_light_multi_camera_fusion/src/traffic_light_multi_camera_fusion_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ -#define TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_MULTI_CAMERA_FUSION_NODE_HPP_ +#define TRAFFIC_LIGHT_MULTI_CAMERA_FUSION_NODE_HPP_ #include @@ -36,7 +36,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { namespace mf = message_filters; @@ -125,5 +125,5 @@ class MultiCameraFusion : public rclcpp::Node */ double message_lifespan_; }; -} // namespace traffic_light -#endif // TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ +} // namespace autoware::traffic_light +#endif // TRAFFIC_LIGHT_MULTI_CAMERA_FUSION_NODE_HPP_ diff --git a/perception/traffic_light_occlusion_predictor/CMakeLists.txt b/perception/autoware_traffic_light_occlusion_predictor/CMakeLists.txt similarity index 56% rename from perception/traffic_light_occlusion_predictor/CMakeLists.txt rename to perception/autoware_traffic_light_occlusion_predictor/CMakeLists.txt index 481561ed92be8..1f494b24b2d77 100644 --- a/perception/traffic_light_occlusion_predictor/CMakeLists.txt +++ b/perception/autoware_traffic_light_occlusion_predictor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(traffic_light_occlusion_predictor) +project(autoware_traffic_light_occlusion_predictor) find_package(autoware_cmake REQUIRED) autoware_package() @@ -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/README.md b/perception/autoware_traffic_light_occlusion_predictor/README.md similarity index 91% rename from perception/traffic_light_occlusion_predictor/README.md rename to perception/autoware_traffic_light_occlusion_predictor/README.md index 2feb07bbe590b..bc57dbea76c97 100644 --- a/perception/traffic_light_occlusion_predictor/README.md +++ b/perception/autoware_traffic_light_occlusion_predictor/README.md @@ -1,8 +1,8 @@ -# The `traffic_light_occlusion_predictor` Package +# The `autoware_traffic_light_occlusion_predictor` Package ## Overview -`traffic_light_occlusion_predictor` receives the detected traffic lights rois and calculates the occlusion ratios of each roi with point cloud. +`autoware_traffic_light_occlusion_predictor` receives the detected traffic lights rois and calculates the occlusion ratios of each roi with point cloud. For each traffic light roi, hundreds of pixels would be selected and projected into the 3D space. Then from the camera point of view, the number of projected pixels that are occluded by the point cloud is counted and used for calculating the occlusion ratio for the roi. As shown in follow image, the red pixels are occluded and the occlusion ratio is the number of red pixels divided by the total pixel numbers. diff --git a/perception/traffic_light_occlusion_predictor/config/traffic_light_occlusion_predictor.param.yaml b/perception/autoware_traffic_light_occlusion_predictor/config/traffic_light_occlusion_predictor.param.yaml similarity index 100% rename from perception/traffic_light_occlusion_predictor/config/traffic_light_occlusion_predictor.param.yaml rename to perception/autoware_traffic_light_occlusion_predictor/config/traffic_light_occlusion_predictor.param.yaml diff --git a/perception/traffic_light_occlusion_predictor/images/occlusion.png b/perception/autoware_traffic_light_occlusion_predictor/images/occlusion.png similarity index 100% rename from perception/traffic_light_occlusion_predictor/images/occlusion.png rename to perception/autoware_traffic_light_occlusion_predictor/images/occlusion.png diff --git a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml b/perception/autoware_traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml similarity index 75% rename from perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml rename to perception/autoware_traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml index aa7e34f8a4a09..d59d5a7717297 100644 --- a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml +++ b/perception/autoware_traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml @@ -7,9 +7,9 @@ - + - + diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/autoware_traffic_light_occlusion_predictor/package.xml similarity index 89% rename from perception/traffic_light_occlusion_predictor/package.xml rename to perception/autoware_traffic_light_occlusion_predictor/package.xml index 313c6e1536e85..78345b26fdfac 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/autoware_traffic_light_occlusion_predictor/package.xml @@ -1,9 +1,9 @@ - traffic_light_occlusion_predictor + autoware_traffic_light_occlusion_predictor 0.1.0 - The traffic_light_occlusion_predictor package + The autoware_traffic_light_occlusion_predictor package Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/autoware_traffic_light_occlusion_predictor/src/node.cpp similarity index 92% rename from perception/traffic_light_occlusion_predictor/src/nodelet.cpp rename to perception/autoware_traffic_light_occlusion_predictor/src/node.cpp index bc668c6af6c3c..8bc11fdea2aad 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/autoware_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/autoware_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/autoware_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/autoware_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/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp similarity index 97% rename from perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp rename to perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 40e50aabf3c17..74878ba0b93af 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/autoware_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/autoware_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/autoware_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/autoware_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/perception/autoware_traffic_light_visualization/CMakeLists.txt b/perception/autoware_traffic_light_visualization/CMakeLists.txt new file mode 100644 index 0000000000000..4db2163e81932 --- /dev/null +++ b/perception/autoware_traffic_light_visualization/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_traffic_light_visualization) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(OpenCV REQUIRED) + +ament_auto_add_library(traffic_light_roi_visualizer SHARED + src/traffic_light_roi_visualizer/node.cpp + src/traffic_light_roi_visualizer/shape_draw.cpp +) + +target_link_libraries(traffic_light_roi_visualizer + ${OpenCV_LIBRARIES} +) + +rclcpp_components_register_node(traffic_light_roi_visualizer + PLUGIN "autoware::traffic_light::TrafficLightRoiVisualizerNode" + EXECUTABLE traffic_light_visualization_node +) + +ament_auto_add_library(traffic_light_map_visualizer SHARED + src/traffic_light_map_visualizer/node.cpp +) + +rclcpp_components_register_node(traffic_light_map_visualizer + PLUGIN "autoware::traffic_light::TrafficLightMapVisualizerNode" + EXECUTABLE traffic_light_map_visualizer_node +) + +# Copy the assets directory to the installation directory +install( + DIRECTORY images/ + DESTINATION share/${PROJECT_NAME}/images +) + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/perception/traffic_light_visualization/README.md b/perception/autoware_traffic_light_visualization/README.md similarity index 85% rename from perception/traffic_light_visualization/README.md rename to perception/autoware_traffic_light_visualization/README.md index 059147e206169..eb0c4678017bd 100644 --- a/perception/traffic_light_visualization/README.md +++ b/perception/autoware_traffic_light_visualization/README.md @@ -1,8 +1,8 @@ -# traffic_light_visualization +# autoware_traffic_light_visualization ## Purpose -The `traffic_light_visualization` is a package that includes two visualizing nodes: +The `autoware_traffic_light_visualization` is a package that includes two visualizing nodes: - **traffic_light_map_visualizer** is a node that shows traffic lights color status and position on rviz as markers. - **traffic_light_roi_visualizer** is a node that draws the result of traffic light recognition nodes (traffic light status, position and classification probability) on the input image as shown in the following figure and publishes it. @@ -55,9 +55,7 @@ None #### Node Parameters -| Name | Type | Default Value | Description | -| ----------------------- | ---- | ------------- | --------------------------------------------------------------- | -| `enable_fine_detection` | bool | false | whether to visualize result of the traffic light fine detection | +{{json_to_markdown("perception/traffic_light_visualization/schema/traffic_light_visualization.schema.json")}} ## Assumptions / Known limits diff --git a/perception/autoware_traffic_light_visualization/config/traffic_light_visualization.param.yaml b/perception/autoware_traffic_light_visualization/config/traffic_light_visualization.param.yaml new file mode 100644 index 0000000000000..1354f7e619b86 --- /dev/null +++ b/perception/autoware_traffic_light_visualization/config/traffic_light_visualization.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + enable_fine_detection: false + use_image_transport: true diff --git a/perception/traffic_light_visualization/images/circle.png b/perception/autoware_traffic_light_visualization/images/circle.png similarity index 100% rename from perception/traffic_light_visualization/images/circle.png rename to perception/autoware_traffic_light_visualization/images/circle.png diff --git a/perception/traffic_light_visualization/images/cross.png b/perception/autoware_traffic_light_visualization/images/cross.png similarity index 100% rename from perception/traffic_light_visualization/images/cross.png rename to perception/autoware_traffic_light_visualization/images/cross.png diff --git a/perception/traffic_light_visualization/images/down_left_arrow.png b/perception/autoware_traffic_light_visualization/images/down_left_arrow.png similarity index 100% rename from perception/traffic_light_visualization/images/down_left_arrow.png rename to perception/autoware_traffic_light_visualization/images/down_left_arrow.png diff --git a/perception/traffic_light_visualization/images/left_arrow.png b/perception/autoware_traffic_light_visualization/images/left_arrow.png similarity index 100% rename from perception/traffic_light_visualization/images/left_arrow.png rename to perception/autoware_traffic_light_visualization/images/left_arrow.png diff --git a/perception/traffic_light_visualization/images/roi-visualization.png b/perception/autoware_traffic_light_visualization/images/roi-visualization.png similarity index 100% rename from perception/traffic_light_visualization/images/roi-visualization.png rename to perception/autoware_traffic_light_visualization/images/roi-visualization.png diff --git a/perception/traffic_light_visualization/images/straight_arrow.png b/perception/autoware_traffic_light_visualization/images/straight_arrow.png similarity index 100% rename from perception/traffic_light_visualization/images/straight_arrow.png rename to perception/autoware_traffic_light_visualization/images/straight_arrow.png diff --git a/perception/traffic_light_visualization/images/unknown.png b/perception/autoware_traffic_light_visualization/images/unknown.png similarity index 100% rename from perception/traffic_light_visualization/images/unknown.png rename to perception/autoware_traffic_light_visualization/images/unknown.png diff --git a/perception/traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml b/perception/autoware_traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml similarity index 76% rename from perception/traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml rename to perception/autoware_traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml index 9a9e7fc02a46d..8ff56915766aa 100644 --- a/perception/traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml +++ b/perception/autoware_traffic_light_visualization/launch/traffic_light_map_visualizer.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml b/perception/autoware_traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml similarity index 75% rename from perception/traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml rename to perception/autoware_traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml index 5c13f8585c774..d4af7a27636df 100644 --- a/perception/traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml +++ b/perception/autoware_traffic_light_visualization/launch/traffic_light_roi_visualizer.launch.xml @@ -5,13 +5,15 @@ + - + + diff --git a/perception/traffic_light_visualization/package.xml b/perception/autoware_traffic_light_visualization/package.xml similarity index 88% rename from perception/traffic_light_visualization/package.xml rename to perception/autoware_traffic_light_visualization/package.xml index ee113a34f10e4..880d8354b2529 100644 --- a/perception/traffic_light_visualization/package.xml +++ b/perception/autoware_traffic_light_visualization/package.xml @@ -1,9 +1,9 @@ - traffic_light_visualization + autoware_traffic_light_visualization 0.1.0 - The traffic_light_visualization package + The autoware_traffic_light_visualization package Yukihiro Saito Tao Zhong Apache License 2.0 diff --git a/perception/autoware_traffic_light_visualization/schema/traffic_light_visualization.schema.json b/perception/autoware_traffic_light_visualization/schema/traffic_light_visualization.schema.json new file mode 100644 index 0000000000000..7058e89c3e5ea --- /dev/null +++ b/perception/autoware_traffic_light_visualization/schema/traffic_light_visualization.schema.json @@ -0,0 +1,35 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for traffic_light_visualization", + "type": "object", + "definitions": { + "traffic_light_visualization": { + "type": "object", + "properties": { + "enable_fine_detection": { + "type": "boolean", + "description": "whether to visualize result of the traffic light fine detection", + "default": "false" + }, + "use_image_transport": { + "type": "boolean", + "description": "whether to apply image transport to compress the output debugging image in the traffic light fine detection", + "default": "true" + } + }, + "required": ["enable_fine_detection", "use_image_transport"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/traffic_light_visualization" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp similarity index 95% rename from perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp rename to perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index 95944a29d6092..90b73e643ae8e 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" +#include "node.hpp" #include #include @@ -26,8 +26,6 @@ #include #include -using std::placeholders::_1; - namespace { [[maybe_unused]] void setColor( @@ -105,12 +103,14 @@ void lightAsMarker( } } // namespace -namespace traffic_light +namespace autoware::traffic_light { TrafficLightMapVisualizerNode::TrafficLightMapVisualizerNode( - const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) + const rclcpp::NodeOptions & node_options) +: rclcpp::Node("traffic_light_map_visualizer_node", node_options) { + using std::placeholders::_1; + light_marker_pub_ = create_publisher("~/output/traffic_light", 1); tl_state_sub_ = create_subscription( @@ -209,4 +209,7 @@ void TrafficLightMapVisualizerNode::binMapCallback( lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); aw_tl_reg_elems_ = lanelet::utils::query::autowareTrafficLights(all_lanelets); } -} // namespace traffic_light +} // namespace autoware::traffic_light + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightMapVisualizerNode) diff --git a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.hpp similarity index 81% rename from perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp rename to perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.hpp index 2cf9a98aea443..61b5204b57592 100644 --- a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ -#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ #include #include @@ -26,12 +26,12 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { class TrafficLightMapVisualizerNode : public rclcpp::Node { public: - TrafficLightMapVisualizerNode(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit TrafficLightMapVisualizerNode(const rclcpp::NodeOptions & options); ~TrafficLightMapVisualizerNode() = default; void trafficSignalsCallback( const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr @@ -47,6 +47,6 @@ class TrafficLightMapVisualizerNode : public rclcpp::Node std::vector aw_tl_reg_elems_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#endif // TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp similarity index 80% rename from perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp rename to perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp index df097e64ecd36..7482749630a48 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp @@ -12,51 +12,61 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp" // NOLINT(whitespace/line_length) +#include "node.hpp" -#include "traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp" // NOLINT(whitespace/line_length) +#include "shape_draw.hpp" #include -#include #include #include #include -namespace traffic_light +namespace autoware::traffic_light { -TrafficLightRoiVisualizerNodelet::TrafficLightRoiVisualizerNodelet( - const rclcpp::NodeOptions & options) +TrafficLightRoiVisualizerNode::TrafficLightRoiVisualizerNode(const rclcpp::NodeOptions & options) : Node("traffic_light_roi_visualizer_node", options) { using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; using std::placeholders::_4; - enable_fine_detection_ = this->declare_parameter("enable_fine_detection", false); + enable_fine_detection_ = this->declare_parameter("enable_fine_detection"); + use_image_transport_ = this->declare_parameter("use_image_transport"); if (enable_fine_detection_) { sync_with_rough_roi_.reset(new SyncWithRoughRoi( SyncPolicyWithRoughRoi(10), image_sub_, roi_sub_, rough_roi_sub_, traffic_signals_sub_)); sync_with_rough_roi_->registerCallback( - std::bind(&TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback, this, _1, _2, _3, _4)); + std::bind(&TrafficLightRoiVisualizerNode::imageRoughRoiCallback, this, _1, _2, _3, _4)); } else { sync_.reset(new Sync(SyncPolicy(10), image_sub_, roi_sub_, traffic_signals_sub_)); sync_->registerCallback( - std::bind(&TrafficLightRoiVisualizerNodelet::imageRoiCallback, this, _1, _2, _3)); + std::bind(&TrafficLightRoiVisualizerNode::imageRoiCallback, this, _1, _2, _3)); } using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&TrafficLightRoiVisualizerNodelet::connectCb, this)); + this, get_clock(), 100ms, std::bind(&TrafficLightRoiVisualizerNode::connectCb, this)); - image_pub_ = - image_transport::create_publisher(this, "~/output/image", rclcpp::QoS{1}.get_rmw_qos_profile()); + if (use_image_transport_) { + image_pub_ = image_transport::create_publisher( + this, "~/output/image", rclcpp::QoS{1}.get_rmw_qos_profile()); + } else { + auto qos = rclcpp::QoS(1); + simple_image_pub_ = this->create_publisher("~/output/image", qos); + } } -void TrafficLightRoiVisualizerNodelet::connectCb() +void TrafficLightRoiVisualizerNode::connectCb() { - if (image_pub_.getNumSubscribers() == 0) { + int num_subscribers = 0; + if (use_image_transport_) { + num_subscribers = image_pub_.getNumSubscribers(); + } else { + num_subscribers = simple_image_pub_->get_subscription_count(); + } + if (num_subscribers == 0) { image_sub_.unsubscribe(); traffic_signals_sub_.unsubscribe(); roi_sub_.unsubscribe(); @@ -74,7 +84,7 @@ void TrafficLightRoiVisualizerNodelet::connectCb() } } -bool TrafficLightRoiVisualizerNodelet::createRect( +bool TrafficLightRoiVisualizerNode::createRect( cv::Mat & image, const tier4_perception_msgs::msg::TrafficLightRoi & tl_roi, const cv::Scalar & color) { @@ -89,7 +99,7 @@ bool TrafficLightRoiVisualizerNodelet::createRect( return true; } -bool TrafficLightRoiVisualizerNodelet::createRect( +bool TrafficLightRoiVisualizerNode::createRect( cv::Mat & image, const tier4_perception_msgs::msg::TrafficLightRoi & tl_roi, const ClassificationResult & result) { @@ -111,13 +121,13 @@ bool TrafficLightRoiVisualizerNodelet::createRect( std::string shape_name = extractShapeName(result.label); - drawTrafficLightShape( + visualization::drawTrafficLightShape( image, shape_name, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), color, 16, result.prob); return true; } -void TrafficLightRoiVisualizerNodelet::imageRoiCallback( +void TrafficLightRoiVisualizerNode::imageRoiCallback( const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & input_tl_roi_msg, [[maybe_unused]] const tier4_perception_msgs::msg::TrafficLightArray::ConstSharedPtr & @@ -145,10 +155,14 @@ void TrafficLightRoiVisualizerNodelet::imageRoiCallback( RCLCPP_ERROR( get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } - image_pub_.publish(cv_ptr->toImageMsg()); + if (use_image_transport_) { + image_pub_.publish(cv_ptr->toImageMsg()); + } else { + simple_image_pub_->publish(*cv_ptr->toImageMsg()); + } } -bool TrafficLightRoiVisualizerNodelet::getClassificationResult( +bool TrafficLightRoiVisualizerNode::getClassificationResult( int id, const tier4_perception_msgs::msg::TrafficLightArray & traffic_signals, ClassificationResult & result) { @@ -171,7 +185,7 @@ bool TrafficLightRoiVisualizerNodelet::getClassificationResult( return has_correspond_traffic_signal; } -bool TrafficLightRoiVisualizerNodelet::getRoiFromId( +bool TrafficLightRoiVisualizerNode::getRoiFromId( int id, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & rois, tier4_perception_msgs::msg::TrafficLightRoi & correspond_roi) { @@ -184,7 +198,7 @@ bool TrafficLightRoiVisualizerNodelet::getRoiFromId( return false; } -void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( +void TrafficLightRoiVisualizerNode::imageRoughRoiCallback( const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & input_tl_roi_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & input_tl_rough_roi_msg, @@ -222,9 +236,14 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( RCLCPP_ERROR( get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } - image_pub_.publish(cv_ptr->toImageMsg()); + if (use_image_transport_) { + image_pub_.publish(cv_ptr->toImageMsg()); + } else { + simple_image_pub_->publish(*cv_ptr->toImageMsg()); + } } -} // namespace traffic_light +} // namespace autoware::traffic_light -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightRoiVisualizerNodelet) +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::traffic_light::TrafficLightRoiVisualizerNode) diff --git a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp similarity index 92% rename from perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp rename to perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp index f7ac7471e21ef..47a13389950a5 100644 --- a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.hpp @@ -11,8 +11,8 @@ // 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 TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ -#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#ifndef TRAFFIC_LIGHT_ROI_VISUALIZER__NODE_HPP_ +#define TRAFFIC_LIGHT_ROI_VISUALIZER__NODE_HPP_ #include #include @@ -38,7 +38,7 @@ #include #include -namespace traffic_light +namespace autoware::traffic_light { struct ClassificationResult { @@ -46,10 +46,10 @@ struct ClassificationResult std::string label; }; -class TrafficLightRoiVisualizerNodelet : public rclcpp::Node +class TrafficLightRoiVisualizerNode : public rclcpp::Node { public: - explicit TrafficLightRoiVisualizerNodelet(const rclcpp::NodeOptions & options); + explicit TrafficLightRoiVisualizerNode(const rclcpp::NodeOptions & options); void connectCb(); void imageRoiCallback( @@ -121,6 +121,8 @@ class TrafficLightRoiVisualizerNodelet : public rclcpp::Node message_filters::Subscriber rough_roi_sub_; message_filters::Subscriber traffic_signals_sub_; image_transport::Publisher image_pub_; + rclcpp::Publisher::SharedPtr simple_image_pub_; + typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::msg::Image, tier4_perception_msgs::msg::TrafficLightRoiArray, tier4_perception_msgs::msg::TrafficLightArray> @@ -136,8 +138,9 @@ class TrafficLightRoiVisualizerNodelet : public rclcpp::Node std::shared_ptr sync_with_rough_roi_; bool enable_fine_detection_; + bool use_image_transport_; }; -} // namespace traffic_light +} // namespace autoware::traffic_light -#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#endif // TRAFFIC_LIGHT_ROI_VISUALIZER__NODE_HPP_ diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp similarity index 95% rename from perception/traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp rename to perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp index 91a00a940885c..c44b1415a09dc 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp @@ -11,19 +11,21 @@ // 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 "traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp" +#include "shape_draw.hpp" #include "opencv2/core/types.hpp" #include "opencv2/highgui.hpp" #include "opencv2/imgproc.hpp" +namespace autoware::traffic_light::visualization +{ void drawShape( const DrawFunctionParams & params, const std::string & filename, bool flipHorizontally, bool flipVertically, int x_offset, int y_offset, double scale_factor) { std::string filepath = - ament_index_cpp::get_package_share_directory("traffic_light_visualization") + "/images/" + - filename; + ament_index_cpp::get_package_share_directory("autoware_traffic_light_visualization") + + "/images/" + filename; cv::Mat shapeImg = cv::imread(filepath, cv::IMREAD_UNCHANGED); if (shapeImg.empty()) { std::cerr << "Failed to load image: " << filepath << std::endl; @@ -171,3 +173,5 @@ void drawTrafficLightShape( std::cerr << "Unknown shape: " << shape << std::endl; } } + +} // namespace autoware::traffic_light::visualization diff --git a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp similarity index 94% rename from perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp rename to perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp index 7f83104edf48e..150d78f767ee3 100644 --- a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.hpp @@ -23,6 +23,8 @@ #include #include +namespace autoware::traffic_light::visualization +{ struct DrawFunctionParams { cv::Mat & image; @@ -49,3 +51,5 @@ void drawUnknown(const DrawFunctionParams & params); void drawTrafficLightShape( cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color, int size, float probability); + +} // namespace autoware::traffic_light::visualization diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp deleted file mode 100644 index c8318f79bbeed..0000000000000 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2022 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 IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ -#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ - -#include -#include - -#include -#include - -namespace image_projection_based_fusion -{ -class PointPaintingTRT : public centerpoint::CenterPointTRT -{ -public: - using centerpoint::CenterPointTRT::CenterPointTRT; - - explicit PointPaintingTRT( - const centerpoint::NetworkParam & encoder_param, const centerpoint::NetworkParam & head_param, - const centerpoint::DensificationParam & densification_param, - const centerpoint::CenterPointConfig & config); - -protected: - bool preprocess( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const tf2_ros::Buffer & tf_buffer) override; - - std::unique_ptr vg_ptr_pp_{nullptr}; -}; -} // namespace image_projection_based_fusion - -#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTPAINTING_TRT_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt deleted file mode 100644 index 68d9e820a57d5..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/CMakeLists.txt +++ /dev/null @@ -1,87 +0,0 @@ -# Copyright 2021-2022 Arm Ltd. -# -# 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. - -cmake_minimum_required(VERSION 3.14) -project(lidar_apollo_segmentation_tvm) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(tvm_runtime_DIR ${tvm_vendor_DIR}) -find_package(tvm_runtime CONFIG REQUIRED) -find_package(PCL 1.10 REQUIRED) - -# Used in "extras" template file -set(${PROJECT_NAME}_BUILT FALSE) - -# Gather neural network information. -set(${PROJECT_NAME}_BACKEND llvm CACHE STRING "${PROJECT_NAME} neural network backend") -set(MODEL_NAME baidu_cnn) - -# Get neural network. -set(NN_DEPENDENCY "") -get_neural_network(${MODEL_NAME} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) -if(NOT NN_DEPENDENCY STREQUAL "") - # Library - ament_auto_add_library(${PROJECT_NAME} SHARED - data/models/${MODEL_NAME}/inference_engine_tvm_config.hpp - include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp - include/lidar_apollo_segmentation_tvm/cluster2d.hpp - include/lidar_apollo_segmentation_tvm/disjoint_set.hpp - include/lidar_apollo_segmentation_tvm/feature_generator.hpp - include/lidar_apollo_segmentation_tvm/feature_map.hpp - include/lidar_apollo_segmentation_tvm/log_table.hpp - include/lidar_apollo_segmentation_tvm/util.hpp - src/lidar_apollo_segmentation_tvm.cpp - src/cluster2d.cpp - src/feature_generator.cpp - src/feature_map.cpp - src/log_table.cpp - ) - add_dependencies(${PROJECT_NAME} ${NN_DEPENDENCY}) - - target_compile_options(${PROJECT_NAME} PRIVATE "-Wno-sign-conversion" "-Wno-conversion") - - # Set includes as "SYSTEM" to avoid compiler errors on their headers. - target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - "${PCL_INCLUDE_DIRS}" - "${tvm_vendor_INCLUDE_DIRS}" - ) - - target_include_directories(${PROJECT_NAME} PRIVATE - data/models - ) - - target_link_libraries(${PROJECT_NAME} - ${PCL_LIBRARIES} - ${tf2_ros_LIBRARIES} - ${tvm_runtime_LIBRARIES} - ) - - if(BUILD_TESTING) - # gtest - set(LIDAR_APOLLO_SEGMENTATION_TVM_GTEST lidar_apollo_segmentation_tvm_gtest) - ament_add_gtest(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} test/main.cpp TIMEOUT 120) - target_include_directories(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} PRIVATE "include") - target_link_libraries(${LIDAR_APOLLO_SEGMENTATION_TVM_GTEST} ${PROJECT_NAME}) - endif() - - ament_export_include_directories(${PCL_INCLUDE_DIRS}) - set(${PROJECT_NAME}_BUILT TRUE) -else() - message(WARNING "Neural network not found, skipping package.") -endif() - -list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake.in") -ament_auto_package() diff --git a/perception/lidar_apollo_segmentation_tvm/README.md b/perception/lidar_apollo_segmentation_tvm/README.md deleted file mode 100644 index 72fb26105e63e..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/README.md +++ /dev/null @@ -1,34 +0,0 @@ -# lidar_apollo_segmentation_tvm - -## Design - -### Usage - -#### Neural network - -This package will not run without a neural network for its inference. -The network is provided by ansible script during the installation of Autoware or can be downloaded manually according to [Manual Downloading](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). -This package uses 'get_neural_network' function from tvm_utility package to create and provide proper dependency. -See its design page for more information on how to handle user-compiled networks. - -#### Backend - -The backend used for the inference can be selected by setting the `lidar_apollo_segmentation_tvm_BACKEND` cmake variable. -The current available options are `llvm` for a CPU backend, and `vulkan` for a GPU backend. -It defaults to `llvm`. - -### Convolutional Neural Networks (CNN) Segmentation - -See the [original design](https://github.com/ApolloAuto/apollo/blob/3422a62ce932cb1c0c269922a0f1aa59a290b733/docs/specs/3d_obstacle_perception.md#convolutional-neural-networks-cnn-segmentation) by Apollo. -The paragraph of interest goes up to, but excluding, the "MinBox Builder" paragraph. -This package instead relies on further processing by a dedicated shape estimator. - -Note: the parameters described in the original design have been modified and are out of date. - -### Inputs / Outputs / API - -The package exports a boolean `lidar_apollo_segmentation_tvm_BUILT` cmake variable. - -## Reference - -Lidar segmentation is based off a core algorithm by [Apollo](https://github.com/ApolloAuto/apollo/blob/r6.0.0/docs/specs/3d_obstacle_perception.md), with modifications from [TIER IV] () for the TVM backend. diff --git a/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp b/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp deleted file mode 100644 index 229a11645a215..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2021 Arm Limited and Contributors. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "tvm_utility/pipeline.hpp" - -#ifndef PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT -#define PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT - -// cspell: ignore bcnn - -namespace model_zoo -{ -namespace perception -{ -namespace lidar_obstacle_detection -{ -namespace baidu_cnn -{ -namespace onnx_bcnn -{ - -static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ - {3, 0, 0}, // modelzoo_version - - "baidu_cnn", // network_name - "llvm", // network_backend - - "./deploy_lib.so", // network_module_path - "./deploy_graph.json", // network_graph_path - "./deploy_param.params", // network_params_path - - // cspell: ignore DLCPU - kDLCPU, // tvm_device_type - 0, // tvm_device_id - - {{"data", kDLFloat, 32, 1, {1, 4, 864, 864}}}, // network_inputs - - {{"deconv0", kDLFloat, 32, 1, {1, 12, 864, 864}}} // network_outputs -}; - -} // namespace onnx_bcnn -} // namespace baidu_cnn -} // namespace lidar_obstacle_detection -} // namespace perception -} // namespace model_zoo -// NOLINTNEXTLINE -#endif // PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp deleted file mode 100644 index 170fe7feaa876..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp +++ /dev/null @@ -1,176 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., TierIV, Autoware Foundation, The Apollo Authors -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ - -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Internal obstacle classification categories. -enum class MetaType { - META_UNKNOWN, - META_SMALL_MOT, - META_BIG_MOT, - META_NON_MOT, - META_PEDESTRIAN, - MAX_META_TYPE -}; - -/// \brief Internal obstacle representation. -struct Obstacle -{ - std::vector grids; - pcl::PointCloud::Ptr cloud_ptr; - float score; - float height; - float heading; - MetaType meta_type; - std::vector meta_type_probabilities; - - Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN) - { - cloud_ptr.reset(new pcl::PointCloud); - meta_type_probabilities.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); - } -}; - -/// \brief Handle the ouput of the CNN-based prediction by obtaining information on individual -/// cells. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D -{ -public: - /// \brief Constructor - /// \param[in] rows The number of rows in the cluster. - /// \param[in] cols The number of columns in the cluster. - /// \param[in] range Scaling factor. - explicit Cluster2D(int32_t rows, int32_t cols, float range); - - /// \brief Construct a directed graph and search the connected components for candidate object - /// clusters. - /// \param[in] inferred_data Prediction information from the neural network inference. - /// \param[in] pc_ptr Input point cloud. - /// \param[in] valid_indices Indices of the points to consider in the point cloud. - /// \param[in] objectness_thresh Threshold for filtering out non-object cells. - /// \param[in] use_all_grids_for_clustering - void cluster( - const float * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, - const pcl::PointIndices & valid_indices, float objectness_thresh, - bool use_all_grids_for_clustering); - - /// \brief Populate the fields of obstacles_ elements. - /// \param[in] inferred_data Prediction information from the neural network inference. - void filter(const float * inferred_data); - - /// \brief Assign a classification type to the obstacles_ elements. - /// \param[in] inferred_data Prediction information from the neural network inference. - void classify(const float * inferred_data); - - /// \brief Remove the candidate clusters that don't meet the parameters' requirements. - /// \param[in] confidence_thresh The detection confidence score threshold. - /// \param[in] height_thresh If it is non-negative, the points that are higher than the predicted - /// object height by height_thresh are filtered out. - /// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed. - /// \return The detected objects. - std::shared_ptr getObjects( - float confidence_thresh, float height_thresh, int32_t min_pts_num); - - /// \brief Transform an obstacle from the internal representation to the external one. - /// \param[in] in_obstacle - /// \return Output obstacle. - tier4_perception_msgs::msg::DetectedObjectWithFeature obstacleToObject( - const Obstacle & in_obstacle) const; - -private: - const int32_t rows_; - const int32_t cols_; - const int32_t siz_; - const float range_; - const float scale_; - const float inv_res_x_; - const float inv_res_y_; - std::vector point2grid_; - std::vector obstacles_; - std::vector id_img_; - - pcl::PointCloud::ConstPtr pc_ptr_; - const std::vector * valid_indices_in_pc_ = nullptr; - - /// \brief Node of a directed graph. - struct Node - { - Node * center_node; - Node * parent; - int8_t node_rank; - int8_t traversed; - bool is_center; - bool is_object; - int32_t point_num; - int32_t obstacle_id; - - Node() - { - center_node = nullptr; - parent = nullptr; - node_rank = 0; - traversed = 0; - is_center = false; - is_object = false; - point_num = 0; - obstacle_id = -1; - } - }; - - /// \brief Check whether a signed row and column values are valid array indices. - inline bool IsValidRowCol(int32_t row, int32_t col) const - { - return IsValidRow(row) && IsValidCol(col); - } - - /// \brief Check whether a signed row value is a valid array index. - inline bool IsValidRow(int32_t row) const { return row >= 0 && row < rows_; } - - /// \brief Check whether a signed column value is a valid array index. - inline bool IsValidCol(int32_t col) const { return col >= 0 && col < cols_; } - - /// \brief Transform a row and column coordinate to a linear grid index. - inline int32_t RowCol2Grid(int32_t row, int32_t col) const { return row * cols_ + col; } - - /// \brief Traverse the directed graph until visiting a node. - /// \param[in] x Node to visit. - void traverse(Node * x) const; -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp deleted file mode 100644 index b86243a7e37e9..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/disjoint_set.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., The Apollo Authors -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -/// \brief Add a new element in a new set. -/// \param[inout] x The element to be added. -template -void DisjointSetMakeSet(T * x) -{ - x->parent = x; - x->node_rank = 0; -} - -/// \brief Recursively follow the chain of parent pointers from the input until reaching a root. -/// \param[inout] x The element which root is looked for. -/// \return The root of the set containing x. -template -T * DisjointSetFindRecursive(T * x) -{ - if (x->parent != x) { - x->parent = DisjointSetFindRecursive(x->parent); - } - return x->parent; -} - -/// \brief Find the root of the set x belongs to. -/// \param[inout] x The set element. -/// \return The root of the set containing x. -template -T * DisjointSetFind(T * x) -{ - T * y = x->parent; - if (y == x || y->parent == y) { - return y; - } - T * root = DisjointSetFindRecursive(y->parent); - x->parent = root; - y->parent = root; - return root; -} - -/// \brief Replace the set containing x and the set containing y with their union. -/// \param[inout] x An element of a first set. -/// \param[inout] y An element of a second set. -template -void DisjointSetUnion(T * x, T * y) -{ - x = DisjointSetFind(x); - y = DisjointSetFind(y); - if (x == y) { - return; - } - if (x->node_rank < y->node_rank) { - x->parent = y; - } else if (y->node_rank < x->node_rank) { - y->parent = x; - } else { - y->parent = x; - x->node_rank++; - } -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__DISJOINT_SET_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp deleted file mode 100644 index 84cf7957d2e7e..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_generator.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ - -#include -#include -#include - -#include - -#include -#include - -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief A FeatureMap generator based on channel feature information. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL FeatureGenerator -{ -private: - const float min_height_; - const float max_height_; - std::shared_ptr map_ptr_; - -public: - /// \brief Constructor - /// \param[in] width The number of cells in X (column) axis of the 2D grid. - /// \param[in] height The number of cells in Y (row) axis of the 2D grid. - /// \param[in] range The range of the 2D grid. - /// \param[in] use_intensity_feature Enable input channel intensity feature. - /// \param[in] use_constant_feature Enable input channel constant feature. - /// \param[in] min_height The minimum height. - /// \param[in] max_height The maximum height. - explicit FeatureGenerator( - int32_t width, int32_t height, int32_t range, bool use_intensity_feature, - bool use_constant_feature, float min_height, float max_height); - - /// \brief Generate a FeatureMap based on the configured features of this object. - /// \param[in] pc_ptr Pointcloud used to populate the generated FeatureMap. - /// \return A shared pointer to the generated FeatureMap. - std::shared_ptr generate( - const pcl::PointCloud::ConstPtr & pc_ptr); -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp deleted file mode 100644 index f78f11c8aed56..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/feature_map.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Abstract interface for FeatureMap. -struct FeatureMapInterface -{ -public: - const int32_t channels; - const int32_t width; - const int32_t height; - const int32_t range; - float * max_height_data; // channel 0 - float * mean_height_data; // channel 1 - float * count_data; // channel 2 - float * direction_data; // channel 3 - float * top_intensity_data; // channel 4 - float * mean_intensity_data; // channel 5 - float * distance_data; // channel 6 - float * nonempty_data; // channel 7 - std::vector map_data; - virtual void initializeMap(std::vector & map) = 0; - virtual void resetMap(std::vector & map) = 0; - explicit FeatureMapInterface(int32_t _channels, int32_t _width, int32_t _height, int32_t _range); - virtual ~FeatureMapInterface() {} -}; - -/// \brief FeatureMap with no extra feature channels. -struct FeatureMap : public FeatureMapInterface -{ - explicit FeatureMap(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; - -/// \brief FeatureMap with an intensity feature channel. -struct FeatureMapWithIntensity : public FeatureMapInterface -{ - explicit FeatureMapWithIntensity(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; - -/// \brief FeatureMap with a constant feature channel. -struct FeatureMapWithConstant : public FeatureMapInterface -{ - explicit FeatureMapWithConstant(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; - -/// \brief FeatureMap with constant and intensity feature channels. -struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface -{ - explicit FeatureMapWithConstantAndIntensity(int32_t width, int32_t height, int32_t range); - void initializeMap(std::vector & map) override; - void resetMap(std::vector & map) override; -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp deleted file mode 100644 index 044f319c4d335..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm.hpp +++ /dev/null @@ -1,200 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ - -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -using tier4_perception_msgs::msg::DetectedObjectsWithFeature; -using tvm_utility::pipeline::TVMArrayContainer; -using tvm_utility::pipeline::TVMArrayContainerVector; - -/// \brief Pre-precessing step of the TVM pipeline. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPreProcessor -: public tvm_utility::pipeline::PreProcessor::ConstPtr> -{ -public: - /// \brief Constructor. - /// \param[in] config The TVM configuration. - /// \param[in] range The range of the 2D grid. - /// \param[in] use_intensity_feature Enable input channel intensity feature. - /// \param[in] use_constant_feature Enable input channel constant feature. - /// \param[in] min_height The minimum height. - /// \param[in] max_height The maximum height. - explicit ApolloLidarSegmentationPreProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, - bool use_intensity_feature, bool use_constant_feature, float min_height, float max_height); - - /// \brief Transfer the input data to a TVM array. - /// \param[in] pc_ptr Input pointcloud. - /// \return A TVM array containing the pointcloud data. - /// \throw std::runtime_error If the features are incorrectly configured. - TVMArrayContainerVector schedule(const pcl::PointCloud::ConstPtr & pc_ptr); - -private: - const int64_t input_channels; - const int64_t input_width; - const int64_t input_height; - const int64_t input_datatype_bytes; - const std::shared_ptr feature_generator; - TVMArrayContainer output; -}; - -/// \brief Post-precessing step of the TVM pipeline. -class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor -: public tvm_utility::pipeline::PostProcessor> -{ -public: - /// \brief Constructor. - /// \param[in] config The TVM configuration. - /// \param[in] pc_ptr Pointcloud containing the initial input information to be matched against - /// the result of the inference. - /// \param[in] range The range of the 2D grid. - /// \param[in] objectness_thresh The threshold of objectness for filtering out non-object cells. - /// \param[in] score_threshold The detection confidence score threshold for filtering out the - /// candidate clusters. - /// \param[in] height_thresh If it is non-negative, the points that are higher than the predicted - /// object height by height_thresh are filtered out. - /// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed. - explicit ApolloLidarSegmentationPostProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, - const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, - float objectness_thresh, float score_threshold, float height_thresh, int32_t min_pts_num); - - /// \brief Copy the inference result. - /// \param[in] input The result of the inference engine. - /// \return The inferred data. - std::shared_ptr schedule(const TVMArrayContainerVector & input); - -private: - const int64_t output_channels; - const int64_t output_width; - const int64_t output_height; - const int64_t output_datatype_bytes; - const float objectness_thresh_; - const float score_threshold_; - const float height_thresh_; - const int32_t min_pts_num_; - const pcl::PointCloud::ConstPtr pc_ptr_; - const std::shared_ptr cluster2d_; -}; - -/// \brief Handle the neural network inference over the input point cloud. -class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation -{ -public: - /// \brief Constructor - /// \param[in] range The range of the 2D grid. - /// \param[in] score_threshold The detection confidence score threshold for filtering out the - /// candidate clusters in the post-processing step. - /// \param[in] use_intensity_feature Enable input channel intensity feature. - /// \param[in] use_constant_feature Enable input channel constant feature. - /// \param[in] z_offset The offset to translate up the input pointcloud before the inference. - /// \param[in] min_height The minimum height. - /// \param[in] max_height The maximum height. - /// \param[in] objectness_thresh The threshold of objectness for filtering out non-object cells in - /// the obstacle clustering step. - /// \param[in] min_pts_num In the post-processing step, the candidate clusters with less than - /// min_pts_num points are removed. - /// \param[in] height_thresh If it is non-negative, the points that are higher than the predicted - /// object height by height_thresh are filtered out in the - /// post-processing step. - /// \param[in] data_path The path to autoware data and artifacts folder - explicit ApolloLidarSegmentation( - int32_t range, float score_threshold, bool use_intensity_feature, bool use_constant_feature, - float z_offset, float min_height, float max_height, float objectness_thresh, - int32_t min_pts_num, float height_thresh, const std::string & data_path); - - /// \brief Detect obstacles. - /// \param[in] input Input pointcloud. - /// \return Detected obstacles. - /// \throw tf2::TransformException If the pointcloud transformation fails. - /// \throw std::runtime_error If the features are incorrectly configured. - std::shared_ptr detectDynamicObjects( - const sensor_msgs::msg::PointCloud2 & input); - - /// \brief Get the name of the neural network used. - /// \return The name. - const std::string & network_name() const; - - /// \brief Check the model's version against supported versions. - /// \return The version status. - tvm_utility::Version version_check() const; - -private: - const float z_offset_; - const pcl::PointCloud::Ptr pcl_pointcloud_ptr_; - // Earliest supported model version. - const std::array model_version_from{2, 0, 0}; - - // Pipeline - using PrePT = ApolloLidarSegmentationPreProcessor; - using IET = tvm_utility::pipeline::InferenceEngineTVM; - using PostPT = ApolloLidarSegmentationPostProcessor; - - const std::shared_ptr PreP; - const std::shared_ptr IE; - const std::shared_ptr PostP; - - const std::shared_ptr> pipeline; - - /// \brief Move up the input pointcloud by z_offset_ and transform the pointcloud to target_frame_ - /// if needed. - /// \param[in] input - /// \param[out] transformed_cloud - /// \throw tf2::TransformException If the pointcloud transformation fails. - void LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL transformCloud( - const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, - float z_offset); - - rclcpp::Clock::SharedPtr clock_ = std::make_shared(RCL_ROS_TIME); - std::unique_ptr tf_buffer_ = std::make_unique(clock_); - - const std::string target_frame_ = "base_link"; -}; -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp deleted file mode 100644 index 85502267090e2..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/log_table.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Use a lookup table to compute the natural logarithm of 1+num. -/// \param[in] num -/// \return ln(1+num) -float calcApproximateLog(float num); -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp deleted file mode 100644 index e8e53d0bb30f2..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/util.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., Autoware Foundation, The Apollo Authors -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ - -/// \brief Project a point from a pointcloud to a 2D map. -/// \param[in] val Coordinate of the point in the pointcloud. -/// \param[in] ori Diameter of area containing the pointcloud. -/// \param[in] scale Scaling factor from pointcloud size to grid size. -/// \return The grid in which the point is. -inline int32_t F2I(float val, float ori, float scale) -{ - return static_cast(std::floor((ori - val) * scale)); -} - -/// \brief Transform a pointcloud scale to a pixel scale. -/// \param[in] in_pc Coordinate of the point in the pointcloud. -/// \param[in] in_range Range of the pointcloud. -/// \param[in] out_size Size of the grid. -/// \return The distance to the point in pixel scale. -inline int32_t Pc2Pixel(float in_pc, float in_range, float out_size) -{ - float inv_res = 0.5f * out_size / in_range; - return static_cast(std::floor((in_range - in_pc) * inv_res)); -} - -/// \brief Transform a pixel scale to a pointcloud scale. -/// \param[in] in_pixel Coordinate of the cell in the grid. -/// \param[in] in_size Size of the grid. -/// \param[in] out_range Range of the pointcloud. -/// \return The distance to the cell in pointcloud scale. -inline float Pixel2Pc(int32_t in_pixel, float in_size, float out_range) -{ - float res = 2.0f * out_range / in_size; - return out_range - (static_cast(in_pixel) + 0.5f) * res; -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware - -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__UTIL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp deleted file mode 100644 index 9696e7983c2ad..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021-2022 Arm Ltd. -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(LIDAR_APOLLO_SEGMENTATION_TVM_BUILDING_DLL) || \ - defined(LIDAR_APOLLO_SEGMENTATION_TVM_EXPORTS) -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __declspec(dllexport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL -#else -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __declspec(dllimport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL -#endif -#elif defined(__linux__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // LIDAR_APOLLO_SEGMENTATION_TVM__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm-extras.cmake.in b/perception/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm-extras.cmake.in deleted file mode 100644 index 2ecef15eb4d26..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/lidar_apollo_segmentation_tvm-extras.cmake.in +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2022 Arm Ltd. -# -# 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. - -set(lidar_apollo_segmentation_tvm_BUILT @lidar_apollo_segmentation_tvm_BUILT@) diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml deleted file mode 100755 index ca4f3d2645cb9..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - lidar_apollo_segmentation_tvm - 0.1.0 - lidar_apollo_segmentation_tvm - Ambroise Vincent - Yoshi Ri - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - libpcl-all-dev - - autoware_perception_msgs - autoware_universe_utils - geometry_msgs - pcl_conversions - rclcpp - sensor_msgs - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_perception_msgs - tvm_utility - tvm_vendor - - ament_cmake_gtest - ament_lint_auto - autoware_lint_common - libpcl-all-dev - - - ament_cmake - - diff --git a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp deleted file mode 100644 index db2068b978a2e..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp +++ /dev/null @@ -1,361 +0,0 @@ -// Copyright 2017-2022 Arm Ltd., TierIV, Autoware Foundation, The Apollo Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include -#include - -using Point = geometry_msgs::msg::Point32; - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y) -{ - tf2::Quaternion q; - q.setRPY(r, p, y); - return tf2::toMsg(q); -} - -Cluster2D::Cluster2D(const int32_t rows, const int32_t cols, const float range) -: rows_(rows), - cols_(cols), - siz_(rows * cols), - range_(range), - scale_(0.5f * static_cast(rows) / range), - inv_res_x_(0.5f * static_cast(cols) / range), - inv_res_y_(0.5f * static_cast(rows) / range) -{ - point2grid_.clear(); - id_img_.assign(siz_, -1); - pc_ptr_.reset(); - valid_indices_in_pc_ = nullptr; -} - -void Cluster2D::traverse(Node * x) const -{ - std::vector p; - p.clear(); - - while (x->traversed == 0) { - p.push_back(x); - x->traversed = 2; - x = x->center_node; - } - if (x->traversed == 2) { - for (int i = static_cast(p.size()) - 1; i >= 0 && p[i] != x; i--) { - p[i]->is_center = true; - } - x->is_center = true; - } - for (size_t i = 0; i < p.size(); i++) { - Node * y = p[i]; - y->traversed = 1; - y->parent = x->parent; - } -} - -void Cluster2D::cluster( - const float * inferred_data, const pcl::PointCloud::ConstPtr & pc_ptr, - const pcl::PointIndices & valid_indices, float objectness_thresh, - bool use_all_grids_for_clustering) -{ - const float * category_pt_data = inferred_data; - const float * instance_pt_x_data = inferred_data + siz_; - const float * instance_pt_y_data = inferred_data + siz_ * 2; - - pc_ptr_ = pc_ptr; - - std::vector> nodes(rows_, std::vector(cols_, Node())); - - valid_indices_in_pc_ = &(valid_indices.indices); - point2grid_.assign(valid_indices_in_pc_->size(), -1); - - for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) { - int32_t point_id = valid_indices_in_pc_->at(i); - const auto & point = pc_ptr_->points[point_id]; - // * the coordinates of x and y have been exchanged in feature generation - // step, - // so we swap them back here. - int32_t pos_x = F2I(point.y, range_, inv_res_x_); // col - int32_t pos_y = F2I(point.x, range_, inv_res_y_); // row - if (IsValidRowCol(pos_y, pos_x)) { - point2grid_[i] = RowCol2Grid(pos_y, pos_x); - nodes[pos_y][pos_x].point_num++; - } - } - - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - int32_t grid = RowCol2Grid(row, col); - Node * node = &nodes[row][col]; - DisjointSetMakeSet(node); - node->is_object = (use_all_grids_for_clustering || nodes[row][col].point_num > 0) && - (*(category_pt_data + grid) >= objectness_thresh); - int32_t center_row = - row + static_cast(std::round(instance_pt_x_data[grid] * scale_)); - int32_t center_col = - col + static_cast(std::round(instance_pt_y_data[grid] * scale_)); - center_row = std::min(std::max(center_row, 0), rows_ - 1); - center_col = std::min(std::max(center_col, 0), cols_ - 1); - node->center_node = &nodes[center_row][center_col]; - } - } - - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - Node * node = &nodes[row][col]; - if (node->is_object && node->traversed == 0) { - traverse(node); - } - } - } - - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - Node * node = &nodes[row][col]; - if (!node->is_center) { - continue; - } - for (int32_t row2 = row - 1; row2 <= row + 1; ++row2) { - for (int32_t col2 = col - 1; col2 <= col + 1; ++col2) { - if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2)) { - Node * node2 = &nodes[row2][col2]; - if (node2->is_center) { - DisjointSetUnion(node, node2); - } - } - } - } - } - } - - int32_t count_obstacles = 0; - obstacles_.clear(); - id_img_.assign(siz_, -1); - for (int32_t row = 0; row < rows_; ++row) { - for (int32_t col = 0; col < cols_; ++col) { - Node * node = &nodes[row][col]; - if (!node->is_object) { - continue; - } - Node * root = DisjointSetFind(node); - if (root->obstacle_id < 0) { - root->obstacle_id = count_obstacles++; - obstacles_.push_back(Obstacle()); - } - int32_t grid = RowCol2Grid(row, col); - id_img_[grid] = root->obstacle_id; - obstacles_[root->obstacle_id].grids.push_back(grid); - } - } - filter(inferred_data); - classify(inferred_data); -} - -void Cluster2D::filter(const float * inferred_data) -{ - const float * confidence_pt_data = inferred_data + siz_ * 3; - const float * heading_pt_x_data = inferred_data + siz_ * 9; - const float * heading_pt_y_data = inferred_data + siz_ * 10; - const float * height_pt_data = inferred_data + siz_ * 11; - - for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { - Obstacle * obs = &obstacles_[obstacle_id]; - double score = 0.0; - double height = 0.0; - double vec_x = 0.0; - double vec_y = 0.0; - for (int32_t grid : obs->grids) { - score += static_cast(confidence_pt_data[grid]); - height += static_cast(height_pt_data[grid]); - vec_x += heading_pt_x_data[grid]; - vec_y += heading_pt_y_data[grid]; - } - obs->score = static_cast(score / static_cast(obs->grids.size())); - obs->height = static_cast(height / static_cast(obs->grids.size())); - obs->heading = static_cast(std::atan2(vec_y, vec_x) * 0.5); - obs->cloud_ptr.reset(new pcl::PointCloud); - } -} - -void Cluster2D::classify(const float * inferred_data) -{ - const float * classify_pt_data = inferred_data + siz_ * 4; - int num_classes = static_cast(MetaType::MAX_META_TYPE); - for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) { - Obstacle * obs = &obstacles_[obs_id]; - - for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { - int32_t grid = obs->grids[grid_id]; - for (int k = 0; k < num_classes; k++) { - obs->meta_type_probabilities[k] += classify_pt_data[k * siz_ + grid]; - } - } - int meta_type_id = 0; - for (int k = 0; k < num_classes; k++) { - obs->meta_type_probabilities[k] /= static_cast(obs->grids.size()); - if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) { - meta_type_id = k; - } - } - obs->meta_type = static_cast(meta_type_id); - } -} - -tier4_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObject( - const Obstacle & in_obstacle) const -{ - using autoware_perception_msgs::msg::DetectedObjectKinematics; - using autoware_perception_msgs::msg::ObjectClassification; - - tier4_perception_msgs::msg::DetectedObjectWithFeature resulting_object; - - resulting_object.object.classification.emplace_back( - autoware_perception_msgs::build() - .label(ObjectClassification::UNKNOWN) - .probability(in_obstacle.score)); - if (in_obstacle.meta_type == MetaType::META_PEDESTRIAN) { - resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN; - } else if (in_obstacle.meta_type == MetaType::META_NON_MOT) { - resulting_object.object.classification.front().label = ObjectClassification::MOTORCYCLE; - } else if (in_obstacle.meta_type == MetaType::META_SMALL_MOT) { - resulting_object.object.classification.front().label = ObjectClassification::CAR; - } else if (in_obstacle.meta_type == MetaType::META_BIG_MOT) { - resulting_object.object.classification.front().label = ObjectClassification::BUS; - } else { - resulting_object.object.classification.front().label = ObjectClassification::UNKNOWN; - } - - pcl::PointXYZ min_point; - pcl::PointXYZ max_point; - for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); - ++pit) { - if (pit->x < min_point.x) { - min_point.x = pit->x; - } - if (pit->y < min_point.y) { - min_point.y = pit->y; - } - if (pit->z < min_point.z) { - min_point.z = pit->z; - } - if (pit->x > max_point.x) { - max_point.x = pit->x; - } - if (pit->y > max_point.y) { - max_point.y = pit->y; - } - if (pit->z > max_point.z) { - max_point.z = pit->z; - } - } - - // cluster and ground filtering - pcl::PointCloud cluster; - const float min_height = min_point.z + ((max_point.z - min_point.z) * 0.1f); - for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); - ++pit) { - if (min_height < pit->z) { - cluster.points.push_back(*pit); - } - } - min_point.z = 0.0; - max_point.z = 0.0; - for (auto pit = cluster.points.begin(); pit != cluster.points.end(); ++pit) { - if (pit->z < min_point.z) { - min_point.z = pit->z; - } - if (pit->z > max_point.z) { - max_point.z = pit->z; - } - } - sensor_msgs::msg::PointCloud2 ros_pc; - pcl::toROSMsg(cluster, ros_pc); - resulting_object.feature.cluster = ros_pc; - - // position - const float height = max_point.z - min_point.z; - const float length = max_point.x - min_point.x; - const float width = max_point.y - min_point.y; - resulting_object.object.kinematics.pose_with_covariance.pose.position.x = - min_point.x + length / 2; - resulting_object.object.kinematics.pose_with_covariance.pose.position.y = min_point.y + width / 2; - resulting_object.object.kinematics.pose_with_covariance.pose.position.z = - min_point.z + height / 2; - - resulting_object.object.kinematics.pose_with_covariance.pose.orientation = - getQuaternionFromRPY(0.0, 0.0, in_obstacle.heading); - resulting_object.object.kinematics.orientation_availability = - DetectedObjectKinematics::SIGN_UNKNOWN; - - return resulting_object; -} - -std::shared_ptr Cluster2D::getObjects( - const float confidence_thresh, const float height_thresh, const int32_t min_pts_num) -{ - auto object_array = std::make_shared(); - - for (size_t i = 0; i < point2grid_.size(); ++i) { - int32_t grid = point2grid_[i]; - if (grid < 0) { - continue; - } - - int32_t obstacle_id = id_img_[grid]; - - int32_t point_id = valid_indices_in_pc_->at(i); - - if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) { - if ( - height_thresh < 0 || - pc_ptr_->points[point_id].z <= obstacles_[obstacle_id].height + height_thresh) { - obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]); - } - } - } - - for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { - Obstacle * obs = &obstacles_[obstacle_id]; - if (static_cast(obs->cloud_ptr->size()) < min_pts_num) { - continue; - } - tier4_perception_msgs::msg::DetectedObjectWithFeature out_obj = obstacleToObject(*obs); - object_array->feature_objects.push_back(out_obj); - } - - return object_array; -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp deleted file mode 100644 index fd038efc60ffb..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_generator.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -namespace -{ -inline float normalizeIntensity(float intensity) -{ - return intensity / 255; -} -} // namespace - -FeatureGenerator::FeatureGenerator( - const int32_t width, const int32_t height, const int32_t range, const bool use_intensity_feature, - const bool use_constant_feature, const float min_height, const float max_height) -: min_height_(min_height), max_height_(max_height) -{ - // select feature map type - if (use_constant_feature && use_intensity_feature) { - map_ptr_ = std::make_shared(width, height, range); - } else if (use_constant_feature) { - map_ptr_ = std::make_shared(width, height, range); - } else if (use_intensity_feature) { - map_ptr_ = std::make_shared(width, height, range); - } else { - map_ptr_ = std::make_shared(width, height, range); - } - map_ptr_->initializeMap(map_ptr_->map_data); -} - -std::shared_ptr FeatureGenerator::generate( - const pcl::PointCloud::ConstPtr & pc_ptr) -{ - const double epsilon = 1e-6; - map_ptr_->resetMap(map_ptr_->map_data); - - const int32_t size = map_ptr_->height * map_ptr_->width; - - const float inv_res_x = 0.5f * map_ptr_->width / map_ptr_->range; - const float inv_res_y = 0.5f * map_ptr_->height / map_ptr_->range; - - for (size_t i = 0; i < pc_ptr->points.size(); ++i) { - if (pc_ptr->points[i].z <= min_height_ || max_height_ <= pc_ptr->points[i].z) { - continue; - } - - // x on grid - const int32_t pos_x = static_cast( - std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].y) * inv_res_x)); - // y on grid - const int32_t pos_y = static_cast( - std::floor((static_cast(map_ptr_->range) - pc_ptr->points[i].x) * inv_res_y)); - if (pos_x < 0 || map_ptr_->width <= pos_x || pos_y < 0 || map_ptr_->height <= pos_y) { - continue; - } - - const int32_t idx = pos_y * map_ptr_->width + pos_x; - - if (map_ptr_->max_height_data[idx] < pc_ptr->points[i].z) { - map_ptr_->max_height_data[idx] = pc_ptr->points[i].z; - if (map_ptr_->top_intensity_data != nullptr) { - map_ptr_->top_intensity_data[idx] = normalizeIntensity(pc_ptr->points[i].intensity); - } - } - map_ptr_->mean_height_data[idx] += pc_ptr->points[i].z; - if (map_ptr_->mean_intensity_data != nullptr) { - map_ptr_->mean_intensity_data[idx] += normalizeIntensity(pc_ptr->points[i].intensity); - } - map_ptr_->count_data[idx] += 1.0f; - } - - for (int32_t i = 0; i < size; ++i) { - if (static_cast(map_ptr_->count_data[i]) < epsilon) { - map_ptr_->max_height_data[i] = 0.0f; - } else { - map_ptr_->mean_height_data[i] /= map_ptr_->count_data[i]; - if (map_ptr_->mean_intensity_data != nullptr) { - map_ptr_->mean_intensity_data[i] /= map_ptr_->count_data[i]; - } - map_ptr_->nonempty_data[i] = 1.0f; - } - map_ptr_->count_data[i] = calcApproximateLog(map_ptr_->count_data[i]); - } - return map_ptr_; -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp b/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp deleted file mode 100644 index a310ddae03f77..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/feature_map.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -FeatureMapInterface::FeatureMapInterface( - const int32_t _channels, const int32_t _width, const int32_t _height, const int32_t _range) -: channels(_channels), - width(_width), - height(_height), - range(_range), - max_height_data(nullptr), - mean_height_data(nullptr), - count_data(nullptr), - direction_data(nullptr), - top_intensity_data(nullptr), - mean_intensity_data(nullptr), - distance_data(nullptr), - nonempty_data(nullptr) -{ - map_data.resize(width * height * channels); -} - -FeatureMap::FeatureMap(const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(4, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - nonempty_data = &(map_data[0]) + width * height * 3; -} -void FeatureMap::initializeMap(std::vector & map) -{ - (void)map; -} -void FeatureMap::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} - -FeatureMapWithIntensity::FeatureMapWithIntensity( - const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(6, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - top_intensity_data = &(map_data[0]) + width * height * 3; - mean_intensity_data = &(map_data[0]) + width * height * 4; - nonempty_data = &(map_data[0]) + width * height * 5; -} -void FeatureMapWithIntensity::initializeMap(std::vector & map) -{ - (void)map; -} -void FeatureMapWithIntensity::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - top_intensity_data[i] = 0.0f; - mean_intensity_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} - -FeatureMapWithConstant::FeatureMapWithConstant( - const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(6, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - direction_data = &(map_data[0]) + width * height * 3; - distance_data = &(map_data[0]) + width * height * 4; - nonempty_data = &(map_data[0]) + width * height * 5; -} -void FeatureMapWithConstant::initializeMap(std::vector & map) -{ - (void)map; - for (int32_t row = 0; row < height; ++row) { - for (int32_t col = 0; col < width; ++col) { - int32_t idx = row * width + col; - // * row <-> x, column <-> y - // return the distance from the car to center of the grid. - // Pc means point cloud = real world scale. so transform pixel scale to - // real world scale - float center_x = Pixel2Pc(row, height, range); - float center_y = Pixel2Pc(col, width, range); - // normalization. -0.5~0.5 - direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); - distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; - } - } -} - -void FeatureMapWithConstant::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} - -FeatureMapWithConstantAndIntensity::FeatureMapWithConstantAndIntensity( - const int32_t width, const int32_t height, const int32_t range) -: FeatureMapInterface(8, width, height, range) -{ - max_height_data = &(map_data[0]) + width * height * 0; - mean_height_data = &(map_data[0]) + width * height * 1; - count_data = &(map_data[0]) + width * height * 2; - direction_data = &(map_data[0]) + width * height * 3; - top_intensity_data = &(map_data[0]) + width * height * 4; - mean_intensity_data = &(map_data[0]) + width * height * 5; - distance_data = &(map_data[0]) + width * height * 6; - nonempty_data = &(map_data[0]) + width * height * 7; -} -void FeatureMapWithConstantAndIntensity::initializeMap(std::vector & map) -{ - (void)map; - for (int32_t row = 0; row < height; ++row) { - for (int32_t col = 0; col < width; ++col) { - int32_t idx = row * width + col; - // * row <-> x, column <-> y - // return the distance from the car to center of the grid. - // Pc means point cloud = real world scale. so transform pixel scale to - // real world scale - float center_x = Pixel2Pc(row, height, range); - float center_y = Pixel2Pc(col, width, range); - // normalization. -0.5~0.5 - direction_data[idx] = std::atan2(center_y, center_x) / static_cast(2.0 * M_PI); - distance_data[idx] = std::hypot(center_x, center_y) / 60.0f - 0.5f; - } - } -} - -void FeatureMapWithConstantAndIntensity::resetMap(std::vector & map) -{ - const int32_t size = width * height; - (void)map; - for (int32_t i = 0; i < size; ++i) { - max_height_data[i] = -5.0f; - mean_height_data[i] = 0.0f; - count_data[i] = 0.0f; - top_intensity_data[i] = 0.0f; - mean_intensity_data[i] = 0.0f; - nonempty_data[i] = 0.0f; - } -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp deleted file mode 100644 index 32211bf84efec..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ /dev/null @@ -1,225 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -// cspell: ignore bcnn -using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -ApolloLidarSegmentationPreProcessor::ApolloLidarSegmentationPreProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range, - bool use_intensity_feature, bool use_constant_feature, float min_height, float max_height) -: input_channels(config.network_inputs[0].node_shape[1]), - input_width(config.network_inputs[0].node_shape[2]), - input_height(config.network_inputs[0].node_shape[3]), - input_datatype_bytes(config.network_inputs[0].tvm_dtype_bits / 8), - feature_generator(std::make_shared( - input_width, input_height, range, use_intensity_feature, use_constant_feature, min_height, - max_height)) -{ - // Allocate input variable - std::vector shape_x{1, input_channels, input_width, input_height}; - TVMArrayContainer x{ - shape_x, - config.network_inputs[0].tvm_dtype_code, - config.network_inputs[0].tvm_dtype_bits, - config.network_inputs[0].tvm_dtype_lanes, - config.tvm_device_type, - config.tvm_device_id}; - output = x; -} - -TVMArrayContainerVector ApolloLidarSegmentationPreProcessor::schedule( - const pcl::PointCloud::ConstPtr & pc_ptr) -{ - // generate feature map - std::shared_ptr feature_map_ptr = feature_generator->generate(pc_ptr); - - if (feature_map_ptr->channels < input_channels) { - throw std::runtime_error("schedule: incorrect feature configuration"); - } - - TVMArrayCopyFromBytes( - output.getArray(), feature_map_ptr->map_data.data(), - input_channels * input_height * input_width * input_datatype_bytes); - - return {output}; -} - -ApolloLidarSegmentationPostProcessor::ApolloLidarSegmentationPostProcessor( - const tvm_utility::pipeline::InferenceEngineTVMConfig & config, - const pcl::PointCloud::ConstPtr & pc_ptr, int32_t range, float objectness_thresh, - float score_threshold, float height_thresh, int32_t min_pts_num) -: output_channels(config.network_outputs[0].node_shape[1]), - output_width(config.network_outputs[0].node_shape[2]), - output_height(config.network_outputs[0].node_shape[3]), - output_datatype_bytes(config.network_outputs[0].tvm_dtype_bits / 8), - objectness_thresh_(objectness_thresh), - score_threshold_(score_threshold), - height_thresh_(height_thresh), - min_pts_num_(min_pts_num), - pc_ptr_(pc_ptr), - cluster2d_(std::make_shared(output_width, output_height, range)) -{ -} - -std::shared_ptr ApolloLidarSegmentationPostProcessor::schedule( - const TVMArrayContainerVector & input) -{ - pcl::PointIndices valid_idx; - valid_idx.indices.resize(pc_ptr_->size()); - std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0); - std::vector feature(output_channels * output_width * output_height, 0); - TVMArrayCopyToBytes( - input[0].getArray(), feature.data(), - output_channels * output_width * output_height * output_datatype_bytes); - cluster2d_->cluster( - feature.data(), pc_ptr_, valid_idx, objectness_thresh_, true /*use all grids for clustering*/); - auto object_array = cluster2d_->getObjects(score_threshold_, height_thresh_, min_pts_num_); - - return object_array; -} - -ApolloLidarSegmentation::ApolloLidarSegmentation( - int32_t range, float score_threshold, bool use_intensity_feature, bool use_constant_feature, - float z_offset, float min_height, float max_height, float objectness_thresh, int32_t min_pts_num, - float height_thresh, const std::string & data_path) -: z_offset_(z_offset), - pcl_pointcloud_ptr_(new pcl::PointCloud), - PreP(std::make_shared( - config, range, use_intensity_feature, use_constant_feature, min_height, max_height)), - IE(std::make_shared(config, "lidar_apollo_segmentation_tvm", data_path)), - PostP(std::make_shared( - config, pcl_pointcloud_ptr_, range, objectness_thresh, score_threshold, height_thresh, - min_pts_num)), - pipeline( - std::make_shared>(*PreP, *IE, *PostP)) -{ -} - -void ApolloLidarSegmentation::transformCloud( - const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, - float z_offset) -{ - if (target_frame_ == input.header.frame_id && z_offset == 0) { - transformed_cloud = input; - } else { - pcl::PointCloud in_cluster, transformed_cloud_cluster; - pcl::fromROSMsg(input, in_cluster); - - // transform pointcloud to target_frame - if (target_frame_ != input.header.frame_id) { - geometry_msgs::msg::TransformStamped transform_stamped; - builtin_interfaces::msg::Time time_stamp = input.header.stamp; - const tf2::TimePoint time_point = tf2::TimePoint( - std::chrono::seconds(time_stamp.sec) + std::chrono::nanoseconds(time_stamp.nanosec)); - transform_stamped = - tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point); - Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - autoware::universe_utils::transformPointCloud( - in_cluster, transformed_cloud_cluster, affine_matrix); - transformed_cloud_cluster.header.frame_id = target_frame_; - } else { - transformed_cloud_cluster = in_cluster; - } - - // move pointcloud z_offset in z axis - if (z_offset != 0) { - Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); - Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); - autoware::universe_utils::transformPointCloud( - transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform); - } - - pcl::toROSMsg(transformed_cloud_cluster, transformed_cloud); - } -} - -std::shared_ptr ApolloLidarSegmentation::detectDynamicObjects( - const sensor_msgs::msg::PointCloud2 & input) -{ - // move up pointcloud z_offset in z axis - sensor_msgs::msg::PointCloud2 transformed_cloud; - ApolloLidarSegmentation::transformCloud(input, transformed_cloud, z_offset_); - // convert from ros to pcl - // pcl::fromROSMsg( - // transformed_cloud, *pcl_pointcloud_ptr_); // Manual conversion is needed since intensity - // comes as an uint8_t - - auto pcl_pointcloud = *pcl_pointcloud_ptr_; - pcl_pointcloud.width = input.width; - pcl_pointcloud.height = input.height; - pcl_pointcloud.is_dense = input.is_dense == 1; - pcl_pointcloud.resize(input.width * input.height); - - sensor_msgs::PointCloud2ConstIterator it_x(input, "x"); - sensor_msgs::PointCloud2ConstIterator it_y(input, "y"); - sensor_msgs::PointCloud2ConstIterator it_z(input, "z"); - sensor_msgs::PointCloud2ConstIterator it_intensity(input, "intensity"); - - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { - pcl::PointXYZI point; - point.x = *it_x; - point.y = *it_y; - point.z = *it_z; - point.intensity = static_cast(*it_intensity); - pcl_pointcloud.emplace_back(std::move(point)); - } - - // inference pipeline - auto output = pipeline->schedule(pcl_pointcloud_ptr_); - for (auto & feature_object : output->feature_objects) { - feature_object.feature.cluster.header = input.header; - } - output->header = input.header; - - // move down pointcloud z_offset in z axis - for (auto & feature_object : output->feature_objects) { - sensor_msgs::msg::PointCloud2 updated_cloud; - transformCloud(feature_object.feature.cluster, updated_cloud, -z_offset_); - feature_object.feature.cluster = updated_cloud; - } - - return output; -} - -const std::string & ApolloLidarSegmentation::network_name() const -{ - return config.network_name; -} - -tvm_utility::Version ApolloLidarSegmentation::version_check() const -{ - return IE->version_check(model_version_from); -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp b/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp deleted file mode 100644 index de2f1734e0733..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/src/log_table.cpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm -{ -struct LogTable -{ - std::vector data; - LogTable() - { - data.resize(256 * 10); - for (size_t i = 0; i < data.size(); ++i) { - data[i] = std::log1p(static_cast(i) / 10); - } - } -}; - -static LogTable log_table; - -float calcApproximateLog(float num) -{ - if (num >= 0) { - size_t integer_num = static_cast(num * 10.0f); - if (integer_num < log_table.data.size()) { - return log_table.data[integer_num]; - } - } - return std::log1p(num); -} -} // namespace lidar_apollo_segmentation_tvm -} // namespace perception -} // namespace autoware diff --git a/perception/lidar_apollo_segmentation_tvm/test/main.cpp b/perception/lidar_apollo_segmentation_tvm/test/main.cpp deleted file mode 100644 index 06cff2a67e908..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm/test/main.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright 2021-2022 Arm Ltd. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include - -#include -#include -#include -#include -#include - -using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; -namespace fs = std::filesystem; - -void test_segmentation( - const std::string & data_path, bool use_intensity_feature, bool use_constant_feature, - bool expect_throw) -{ - // Instantiate the pipeline - const int width = 1; - const int height = 10000; - const int range = 70; - const float score_threshold = 0.8f; - const float z_offset = 1.0f; - const float min_height = -5.0f; - const float max_height = 5.0f; - const float objectness_thresh = 0.5f; - const int32_t min_pts_num = 3; - const float height_thresh = 0.5f; - - ApolloLidarSegmentation segmentation( - range, score_threshold, use_intensity_feature, use_constant_feature, z_offset, min_height, - max_height, objectness_thresh, min_pts_num, height_thresh, data_path); - - auto version_status = segmentation.version_check(); - EXPECT_NE(version_status, tvm_utility::Version::Unsupported); - - std::random_device rd; - std::mt19937 gen(42); - std::uniform_real_distribution dis(-50.0, 50.0); - std::vector v(width * height * sizeof(float) * 4); - for (size_t i = 0; i < width * height * 4; i++) { - reinterpret_cast(v.data())[i] = dis(gen); - } - - sensor_msgs::msg::PointCloud2 input{}; - input.header.frame_id = "base_link"; - input.fields.resize(4U); - input.fields[0U].name = "x"; - input.fields[1U].name = "y"; - input.fields[2U].name = "z"; - input.fields[3U].name = "intensity"; - for (uint32_t idx = 0U; idx < 4U; ++idx) { - input.fields[idx].offset = static_cast(idx * sizeof(float)); - input.fields[idx].datatype = sensor_msgs::msg::PointField::FLOAT32; - input.fields[idx].count = 1U; - input.point_step += static_cast(sizeof(float)); - } - input.height = static_cast(height); - input.width = static_cast(width); - input.is_bigendian = false; - input.row_step = input.point_step * input.width; - input.is_dense = false; - input.data = v; - - std::shared_ptr output; - bool has_thrown = false; - try { - output = segmentation.detectDynamicObjects(input); - } catch (const std::exception & e) { - has_thrown = true; - } - EXPECT_EQ(expect_throw, has_thrown); -} - -// Other test configurations to increase code coverage. -TEST(lidar_apollo_segmentation_tvm, others) -{ - std::string home = std::getenv("HOME"); - fs::path data_path(home); - data_path /= "autoware_data"; - fs::path apollo_data_path(data_path); - apollo_data_path /= "lidar_apollo_segmentation_tvm"; - fs::path deploy_path(apollo_data_path); - deploy_path /= "models/baidu_cnn"; - - fs::path deploy_graph("deploy_graph.json"); - fs::path deploy_lib("deploy_lib.so"); - fs::path deploy_param("deploy_param.params"); - - fs::path deploy_graph_path = deploy_path / deploy_graph; - fs::path deploy_lib_path = deploy_path / deploy_lib; - fs::path deploy_param_path = deploy_path / deploy_param; - - if ( - !fs::exists(deploy_graph_path) || !fs::exists(deploy_lib_path) || - !fs::exists(deploy_param_path)) { - printf("Model deploy files not found. Skip test.\n"); - GTEST_SKIP(); - return; - } - test_segmentation(data_path, false, true, false); - test_segmentation(data_path, true, true, false); - test_segmentation(data_path, false, false, false); - test_segmentation(data_path, true, false, false); -} diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt b/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt deleted file mode 100644 index 100e14277ec1b..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -# Copyright 2021-2023 Arm Ltd. -# -# 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. - -cmake_minimum_required(VERSION 3.14) -project(lidar_apollo_segmentation_tvm_nodes) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# Only build if the "core" package has been built. -if(lidar_apollo_segmentation_tvm_BUILT) - # Set lidar_apollo_segmentation_tvm includes as "SYSTEM" to ignore compiler errors on PCL headers - include_directories(SYSTEM "${lidar_apollo_segmentation_tvm_INCLUDE_DIRS}") - - # Library - ament_auto_add_library(${PROJECT_NAME} SHARED - include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp - src/lidar_apollo_segmentation_tvm_node.cpp - ) - target_link_libraries(${PROJECT_NAME} ${lidar_apollo_segmentation_tvm_LIBRARIES}) - - rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "autoware::perception::lidar_apollo_segmentation_tvm_nodes::ApolloLidarSegmentationNode" - EXECUTABLE ${PROJECT_NAME}_exe - ) - - if(BUILD_TESTING) - add_ros_test( - test/launch.test.py - TIMEOUT "30" - ) - endif() - - ament_auto_package(INSTALL_TO_SHARE - launch - config - ) -else() - message(WARNING "lidar_apollo_segmentation_tvm not built, skipping package.") - ament_auto_package() -endif() diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/README.md b/perception/lidar_apollo_segmentation_tvm_nodes/README.md deleted file mode 100644 index a9ffd59f228ec..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/README.md +++ /dev/null @@ -1,83 +0,0 @@ - - -# lidar_apollo_segmentation_tvm_nodes - -## Purpose / Use cases - -An alternative to Euclidean clustering. -This node detects and labels foreground obstacles (e.g. cars, motorcycles, pedestrians) from a point -cloud, using a neural network. - -## Design - -See the design of the algorithm in the core (lidar_apollo_segmentation_tvm) package's design documents. - -### Usage - -`lidar_apollo_segmentation_tvm` and `lidar_apollo_segmentation_tvm_nodes` will not work without a neural network. -See the lidar_apollo_segmentation_tvm usage for more information. - -### Assumptions / Known limits - -The original node from Apollo has a Region Of Interest (ROI) filter. -This has the benefit of working with a filtered point cloud that includes only the points inside the -ROI (i.e., the drivable road and junction areas) with most of the background obstacles removed (such -as buildings and trees around the road region). -Not having this filter may negatively impact performance. - -### Inputs / Outputs / API - -#### Inputs - -The input are non-ground points as a PointCloud2 message from the sensor_msgs package. - -#### Outputs - -The output is a [DetectedObjectsWithFeature](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/object_recognition/DetectedObjectsWithFeature.msg). - -#### Parameters - -{{ json_to_markdown("perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json") }} - -### Error detection and handling - -Abort and warn when the input frame can't be converted to `base_link`. - -### Security considerations - -Both the input and output are controlled by the same actor, so the following security concerns are -out-of-scope: - -- Spoofing -- Tampering - -Leaking data to another actor would require a flaw in TVM or the host operating system that allows -arbitrary memory to be read, a significant security flaw in itself. -This is also true for an external actor operating the pipeline early: only the object that initiated -the pipeline can run the methods to receive its output. - -A Denial-of-Service attack could make the target hardware unusable for other pipelines but would -require being able to run code on the CPU, which would already allow a more severe Denial-of-Service -attack. - -No elevation of privilege is required for this package. - -## Future extensions / Unimplemented parts - -## Related issues - -- #226: Autoware.Auto Neural Networks Inference Architecture Design diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml b/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml deleted file mode 100644 index 30bf0ba68d28c..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/config/lidar_apollo_segmentation_tvm_nodes.param.yaml +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2023 Arm Ltd. -# -# 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. - -/**: - ros__parameters: - range: 90 - score_threshold: 0.1 - use_intensity_feature: false - use_constant_feature: false - z_offset: 0.0 - min_height: -5.0 - max_height: 5.0 - objectness_thresh: 0.5 - min_pts_num: 3 - height_thresh: 0.5 - data_path: $(env HOME)/autoware_data diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp deleted file mode 100644 index eb05ca28c3a39..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/lidar_apollo_segmentation_tvm_node.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2020-2022 Arm Ltd., TierIV -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ - -#include -#include -#include -#include -#include - -#include - -#include - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm_nodes -{ -/// \brief Object detection node based on neural network inference. -class LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC ApolloLidarSegmentationNode : public rclcpp::Node -{ -private: - const rclcpp::Subscription::SharedPtr m_cloud_sub_ptr; - const rclcpp::Publisher::SharedPtr - m_detected_pub_ptr; - const std::shared_ptr m_detector_ptr; - /// \brief Main callback function. - void LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL - pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr & msg); - -public: - /// \brief Constructor - /// \param options Additional options to control creation of the node. - explicit ApolloLidarSegmentationNode(const rclcpp::NodeOptions & options); -}; -} // namespace lidar_apollo_segmentation_tvm_nodes -} // namespace perception -} // namespace autoware -#endif // LIDAR_APOLLO_SEGMENTATION_TVM_NODES__LIDAR_APOLLO_SEGMENTATION_TVM_NODE_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp b/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp deleted file mode 100644 index 0521737a96ed1..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/include/lidar_apollo_segmentation_tvm_nodes/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021-2022 Arm Ltd. -// -// 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 LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ - -#if defined(__WIN32) -#if defined(LIDAR_APOLLO_SEGMENTATION_TVM_NODES_BUILDING_DLL) || \ - defined(LIDAR_APOLLO_SEGMENTATION_TVM_NODES_EXPORTS) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __declspec(dllexport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL -#else -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __declspec(dllimport) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL -#endif -#elif defined(__linux__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_PUBLIC __attribute__((visibility("default"))) -#define LIDAR_APOLLO_SEGMENTATION_TVM_NODES_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // LIDAR_APOLLO_SEGMENTATION_TVM_NODES__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py deleted file mode 100644 index 2d070f2611ac5..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.py +++ /dev/null @@ -1,57 +0,0 @@ -# Copyright 2021-2023 Arm Ltd., 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. - -"""Launch lidar segmentation node.""" - -import os - -from ament_index_python.packages import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -import launch_ros.parameter_descriptions - - -def generate_launch_description(): - param_file = os.path.join( - get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"), - "config/lidar_apollo_segmentation_tvm_nodes.param.yaml", - ) - - lidar_apollo_segmentation_tvm_node_params = launch_ros.parameter_descriptions.ParameterFile( - param_file=param_file, allow_substs=True - ) - - arguments = [ - DeclareLaunchArgument("input/pointcloud", default_value="/sensing/lidar/pointcloud"), - DeclareLaunchArgument("output/objects", default_value="labeled_clusters"), - DeclareLaunchArgument( - "data_path", default_value=os.path.join(os.environ["HOME"], "autoware_data") - ), - ] - - # lidar segmentation node execution definition. - lidar_apollo_segmentation_tvm_node_runner = Node( - package="lidar_apollo_segmentation_tvm_nodes", - executable="lidar_apollo_segmentation_tvm_nodes_exe", - remappings=[ - ("points_in", LaunchConfiguration("input/pointcloud")), - ("objects_out", LaunchConfiguration("output/objects")), - ], - parameters=[lidar_apollo_segmentation_tvm_node_params], - output="screen", - ) - - return launch.LaunchDescription(arguments + [lidar_apollo_segmentation_tvm_node_runner]) diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml b/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml deleted file mode 100644 index c97d7356f5616..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/launch/lidar_apollo_segmentation_tvm_nodes.launch.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/package.xml b/perception/lidar_apollo_segmentation_tvm_nodes/package.xml deleted file mode 100755 index 7d3ac6fbe5ac7..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - lidar_apollo_segmentation_tvm_nodes - 0.1.0 - lidar_apollo_segmentation_tvm_nodes - Ambroise Vincent - Yoshi Ri - Apache 2.0 - - ament_cmake - ament_cmake_auto - autoware_cmake - - lidar_apollo_segmentation_tvm - rclcpp - rclcpp_components - sensor_msgs - - ament_lint_auto - autoware_lint_common - ros_testing - - - ament_cmake - - diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json b/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json deleted file mode 100644 index aaabbf2b65879..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/schema/lidar_apollo_segmentation_tvm_nodes.schema.json +++ /dev/null @@ -1,98 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Lidar Apollo Segmentation TVM Nodes", - "type": "object", - "definitions": { - "lidar_apollo_segmentation_tvm_nodes": { - "type": "object", - "properties": { - "range": { - "type": "integer", - "default": 90, - "exclusiveMinimum": 0, - "description": "The range of the 2D grid with respect to the origin." - }, - "score_threshold": { - "type": "number", - "default": 0.1, - "minimum": 0.0, - "maximum": 1.0, - "description": "The detection confidence score threshold for filtering out the candidate clusters in the post-processing step." - }, - "use_intensity_feature": { - "type": "boolean", - "default": "false", - "description": "Enable input channel intensity feature." - }, - "use_constant_feature": { - "type": "boolean", - "default": "false", - "description": "Enable input channel constant feature." - }, - "z_offset": { - "type": "number", - "default": 0.0, - "description": "Vertical translation of the pointcloud before inference." - }, - "min_height": { - "type": "number", - "default": -5.0, - "description": "The minimum height with respect to the origin" - }, - "max_height": { - "type": "number", - "default": 5.0, - "description": "The maximum height with respect to the origin." - }, - "objectness_thresh": { - "type": "number", - "default": 0.5, - "minimum": 0.0, - "maximum": 1.0, - "description": "The threshold of objectness for filtering out non-object cells in the obstacle clustering step." - }, - "min_pts_num": { - "type": "integer", - "default": 3, - "minimum": 0, - "description": "In the post-processing step, the candidate clusters with less than min_pts_num points are removed." - }, - "height_thresh": { - "type": "number", - "default": 0.5, - "description": "If it is non-negative, the points that are higher than the predicted object height by height_thresh are filtered out in the post-processing step." - }, - "data_path": { - "type": "string", - "default": "$(env HOME)/autoware_data", - "description": "Packages data and artifacts directory path." - } - }, - "required": [ - "range", - "score_threshold", - "use_intensity_feature", - "use_constant_feature", - "z_offset", - "min_height", - "max_height", - "objectness_thresh", - "min_pts_num", - "height_thresh", - "data_path" - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/lidar_apollo_segmentation_tvm_nodes" - } - }, - "required": ["ros__parameters"] - } - }, - "required": ["/**"] -} diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp b/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp deleted file mode 100644 index 618d2e258c99b..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/src/lidar_apollo_segmentation_tvm_node.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2020-2023 Arm Ltd., TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include - -using autoware::perception::lidar_apollo_segmentation_tvm::ApolloLidarSegmentation; - -namespace autoware -{ -namespace perception -{ -namespace lidar_apollo_segmentation_tvm_nodes -{ - -ApolloLidarSegmentationNode::ApolloLidarSegmentationNode(const rclcpp::NodeOptions & options) -: Node("lidar_apollo_segmentation_tvm", options), - m_cloud_sub_ptr{create_subscription( - "points_in", rclcpp::SensorDataQoS().keep_last(1), - [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { pointCloudCallback(msg); })}, - m_detected_pub_ptr{create_publisher( - "objects_out", rclcpp::QoS{1})}, - m_detector_ptr{std::make_shared( - declare_parameter("range"), declare_parameter("score_threshold"), - declare_parameter("use_intensity_feature"), - declare_parameter("use_constant_feature"), declare_parameter("z_offset"), - declare_parameter("min_height"), declare_parameter("max_height"), - declare_parameter("objectness_thresh"), declare_parameter("min_pts_num"), - declare_parameter("height_thresh"), declare_parameter("data_path"))} -{ - // Log unexpected versions of the neural network. - auto version_status = m_detector_ptr->version_check(); - if (version_status != tvm_utility::Version::OK) { - auto network_name = m_detector_ptr->network_name(); - if (version_status == tvm_utility::Version::Unknown) { - RCLCPP_INFO( - get_logger(), "The '%s' network doesn't provide a version number.", network_name.c_str()); - } else if (version_status == tvm_utility::Version::Untested) { - RCLCPP_WARN( - get_logger(), "The version of the '%s' network is untested.", network_name.c_str()); - } else if (version_status == tvm_utility::Version::Unsupported) { - RCLCPP_ERROR( - get_logger(), "The version of the '%s' network is unsupported.", network_name.c_str()); - } - } -} - -void ApolloLidarSegmentationNode::pointCloudCallback( - const sensor_msgs::msg::PointCloud2::SharedPtr & msg) -{ - std::shared_ptr output_msg; - try { - output_msg = m_detector_ptr->detectDynamicObjects(*msg); - } catch (const std::exception & e) { - RCLCPP_WARN(get_logger(), e.what()); - return; - } - m_detected_pub_ptr->publish(*output_msg); -} -} // namespace lidar_apollo_segmentation_tvm_nodes -} // namespace perception -} // namespace autoware - -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::perception::lidar_apollo_segmentation_tvm_nodes::ApolloLidarSegmentationNode) diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py deleted file mode 100644 index 0a9adafc3f641..0000000000000 --- a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py +++ /dev/null @@ -1,60 +0,0 @@ -# Copyright 2021-2022 Arm Ltd. -# -# 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. - -import os -import unittest - -from ament_index_python import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -import launch_testing -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - lidar_apollo_segmentation_tvm = Node( - package="lidar_apollo_segmentation_tvm_nodes", - executable="lidar_apollo_segmentation_tvm_nodes_exe", - name="lidar_apollo_segmentation_tvm_nodes", - namespace="benchmark", - output="screen", - parameters=[ - os.path.join( - get_package_share_directory("lidar_apollo_segmentation_tvm_nodes"), - "param/test.param.yaml", - ) - ], - ) - - context = {"lidar_apollo_segmentation_tvm": lidar_apollo_segmentation_tvm} - - launch_description = LaunchDescription( - [ - lidar_apollo_segmentation_tvm, - # Start tests right away - no need to wait for anything - launch_testing.actions.ReadyToTest(), - ] - ) - - return launch_description, context - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_output, proc_info, lidar_apollo_segmentation_tvm): - # Check that process exits with code -2 or -15 - launch_testing.asserts.assertExitCodes( - proc_info, [-2, -15], process=lidar_apollo_segmentation_tvm - ) diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md deleted file mode 100644 index 97d66bf9f648c..0000000000000 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ /dev/null @@ -1,109 +0,0 @@ -# probabilistic_occupancy_grid_map - -## Purpose - -This package outputs the probability of having an obstacle as occupancy grid map. -![pointcloud_based_occupancy_grid_map_sample_image](./image/pointcloud_based_occupancy_grid_map_sample_image.gif) - -## References/External links - -- [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md) -- [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md) -- [Grid map fusion](synchronized_grid_map_fusion.md) - -## Settings - -Occupancy grid map is generated on `map_frame`, and grid orientation is fixed. - -You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which means sensor origin and gridmap origin respectively. Especially, set your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) as a `scan_origin_frame` would result in better performance. - -![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 | - -### Downsample input pointcloud(Optional) - -If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used. This feature is currently only for the pointcloud based occupancy grid map. - -- pointcloud_based_occupancy_grid_map method - -```yaml -# downsampled raw and obstacle pointcloud -/perception/occupancy_grid_map/obstacle/downsample/pointcloud -/perception/occupancy_grid_map/raw/downsample/pointcloud -``` - -- multi_lidar_pointcloud_based_point_cloud - -```yaml -# downsampled raw and obstacle pointcloud -/perception/occupancy_grid_map/obstacle/downsample/pointcloud -/perception/occupancy_grid_map//raw/downsample/pointcloud -``` - -### Test - -This package provides unit tests using `gtest`. -You can run the test by the following command. - -```bash -colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+ -``` - -Test contains the following. - -- Unit test for cost value conversion function -- Unit test for utility functions -- Unit test for occupancy grid map fusion functions -- Input/Output test for pointcloud based occupancy grid map diff --git a/perception/simple_object_merger/launch/simple_object_merger.launch.xml b/perception/simple_object_merger/launch/simple_object_merger.launch.xml deleted file mode 100644 index 43e22ea081feb..0000000000000 --- a/perception/simple_object_merger/launch/simple_object_merger.launch.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md deleted file mode 100644 index af88e73cc04a6..0000000000000 --- a/perception/tensorrt_yolox/README.md +++ /dev/null @@ -1,197 +0,0 @@ -# tensorrt_yolox - -## Purpose - -This package detects target objects e.g., cars, trucks, bicycles, and pedestrians and segment target objects such as cars, trucks, buses and pedestrian, building, vegetation, road, sidewalk on a image based on [YOLOX](https://github.com/Megvii-BaseDetection/YOLOX) model with multi-header structure. - -## Inner-workings / Algorithms - -### Cite - - - -Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Series in 2021", arXiv preprint arXiv:2107.08430, 2021 [[ref](https://arxiv.org/abs/2107.08430)] - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ---------- | ------------------- | --------------- | -| `in/image` | `sensor_msgs/Image` | The input image | - -### Output - -| Name | Type | Description | -| ---------------- | -------------------------------------------------- | ------------------------------------------------------------------- | -| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | -| `out/image` | `sensor_msgs/Image` | The image with 2D bounding boxes for visualization | -| `out/mask` | `sensor_msgs/Image` | The semantic segmentation mask | -| `out/color_mask` | `sensor_msgs/Image` | The colorized image of semantic segmentation mask for visualization | - -## 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. | - -## Assumptions / Known limits - -The label contained in detected 2D bounding boxes (i.e., `out/objects`) will be either one of the followings: - -- CAR -- PEDESTRIAN ("PERSON" will also be categorized as "PEDESTRIAN") -- BUS -- TRUCK -- BICYCLE -- MOTORCYCLE - -If other labels (case insensitive) are contained in the file specified via the `label_file` parameter, -those are labeled as `UNKNOWN`, while detected rectangles are drawn in the visualization result (`out/image`). - -The semantic segmentation mask is a gray image whose each pixel is index of one following class: - -| index | semantic name | -| ----- | ---------------- | -| 0 | road | -| 1 | building | -| 2 | wall | -| 3 | obstacle | -| 4 | traffic_light | -| 5 | traffic_sign | -| 6 | person | -| 7 | vehicle | -| 8 | bike | -| 9 | road | -| 10 | sidewalk | -| 11 | roadPaint | -| 12 | curbstone | -| 13 | crosswalk_others | -| 14 | vegetation | -| 15 | sky | - -## Onnx model - -A sample model (named `yolox-tiny.onnx`) is downloaded by ansible script on env preparation stage, if not, please, follow [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). -To accelerate Non-maximum-suppression (NMS), which is one of the common post-process after object detection inference, -`EfficientNMS_TRT` module is attached after the ordinal YOLOX (tiny) network. -The `EfficientNMS_TRT` module contains fixed values for `score_threshold` and `nms_threshold` in it, -hence these parameters are ignored when users specify ONNX models including this module. - -This package accepts both `EfficientNMS_TRT` attached ONNXs and [models published from the official YOLOX repository](https://github.com/Megvii-BaseDetection/YOLOX/tree/main/demo/ONNXRuntime#download-onnx-models) (we referred to them as "plain" models). - -In addition to `yolox-tiny.onnx`, a custom model named `yolox-sPlus-opt-pseudoV2-T4-960x960-T4-seg16cls` is either available. -This model is multi-header structure model which is based on YOLOX-s and tuned to perform more accurate detection with almost comparable execution speed with `yolox-tiny`. -To get better results with this model, users are recommended to use some specific running arguments -such as `precision:=int8`, `calibration_algorithm:=Entropy`, `clip_value:=6.0`. -Users can refer `launch/yolox_sPlus_opt.launch.xml` to see how this model can be used. -Beside detection result, this model also output image semantic segmentation result for pointcloud filtering purpose. - -All models are automatically converted to TensorRT format. -These converted files will be saved in the same directory as specified ONNX files -with `.engine` filename extension and reused from the next run. -The conversion process may take a while (**typically 10 to 20 minutes**) and the inference process is blocked -until complete the conversion, so it will take some time until detection results are published (**even until appearing in the topic list**) on the first run - -### Package acceptable model generation - -To convert users' own model that saved in PyTorch's `pth` format into ONNX, -users can exploit the converter offered by the official repository. -For the convenience, only procedures are described below. -Please refer [the official document](https://github.com/Megvii-BaseDetection/YOLOX/tree/main/demo/ONNXRuntime#convert-your-model-to-onnx) for more detail. - -#### For plain models - -1. Install dependency - - ```shell - git clone git@github.com:Megvii-BaseDetection/YOLOX.git - cd YOLOX - python3 setup.py develop --user - ``` - -2. Convert pth into ONNX - - ```shell - python3 tools/export_onnx.py \ - --output-name YOUR_YOLOX.onnx \ - -f YOUR_YOLOX.py \ - -c YOUR_YOLOX.pth - ``` - -#### For EfficientNMS_TRT embedded models - -1. Install dependency - - ```shell - git clone git@github.com:Megvii-BaseDetection/YOLOX.git - cd YOLOX - python3 setup.py develop --user - pip3 install git+ssh://git@github.com/wep21/yolox_onnx_modifier.git --user - ``` - -2. Convert pth into ONNX - - ```shell - python3 tools/export_onnx.py \ - --output-name YOUR_YOLOX.onnx \ - -f YOUR_YOLOX.py \ - -c YOUR_YOLOX.pth - --decode_in_inference - ``` - -3. Embed `EfficientNMS_TRT` to the end of YOLOX - - ```shell - yolox_onnx_modifier YOUR_YOLOX.onnx -o YOUR_YOLOX_WITH_NMS.onnx - ``` - -## Label file - -A sample label file (named `label.txt`) and semantic segmentation color map file (name `semseg_color_map.csv`) are also downloaded automatically during env preparation process -(**NOTE:** This file is incompatible with models that output labels for the COCO dataset (e.g., models from the official YOLOX repository)). - -This file represents the correspondence between class index (integer outputted from YOLOX network) and -class label (strings making understanding easier). This package maps class IDs (incremented from 0) -with labels according to the order in this file. - -## Reference repositories - -- -- -- diff --git a/perception/traffic_light_multi_camera_fusion/CMakeLists.txt b/perception/traffic_light_multi_camera_fusion/CMakeLists.txt deleted file mode 100644 index 5765ebf58fd45..0000000000000 --- a/perception/traffic_light_multi_camera_fusion/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(traffic_light_multi_camera_fusion) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -include_directories( - SYSTEM -) - -ament_auto_add_library(traffic_light_multi_camera_fusion SHARED - src/node.cpp -) - -rclcpp_components_register_node(traffic_light_multi_camera_fusion - PLUGIN "traffic_light::MultiCameraFusion" - EXECUTABLE traffic_light_multi_camera_fusion_node -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/perception/traffic_light_visualization/CMakeLists.txt b/perception/traffic_light_visualization/CMakeLists.txt deleted file mode 100644 index 08f6b48cabbd9..0000000000000 --- a/perception/traffic_light_visualization/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(traffic_light_visualization) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(OpenCV REQUIRED) - -ament_auto_add_library(traffic_light_roi_visualizer_nodelet SHARED - src/traffic_light_roi_visualizer/nodelet.cpp - src/traffic_light_roi_visualizer/shape_draw.cpp -) - -target_link_libraries(traffic_light_roi_visualizer_nodelet - ${OpenCV_LIBRARIES} -) - -rclcpp_components_register_node(traffic_light_roi_visualizer_nodelet - PLUGIN "traffic_light::TrafficLightRoiVisualizerNodelet" - EXECUTABLE traffic_light_visualization_node -) - -ament_auto_add_executable(traffic_light_map_visualizer_node - src/traffic_light_map_visualizer/node.cpp - src/traffic_light_map_visualizer/main.cpp -) - -# Copy the assets directory to the installation directory -install( - DIRECTORY images/ - DESTINATION share/${PROJECT_NAME}/images -) - -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp deleted file mode 100644 index 62d416457c6f0..0000000000000 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2020 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 "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared( - "traffic_light_map_visualizer_node", node_options); - rclcpp::spin(node); - return 0; -} diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 3cccd9fab0b48..b77aa31947980 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -95,11 +95,9 @@ std::shared_ptr findNearestParkinglot( const std::shared_ptr & lanelet_map_ptr, const lanelet::BasicPoint2d & current_position) { - const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); - const auto linked_parking_lot = std::make_shared(); const auto result = lanelet::utils::query::getLinkedParkingLot( - current_position, all_parking_lots, linked_parking_lot.get()); + current_position, lanelet_map_ptr, linked_parking_lot.get()); if (result) { return linked_parking_lot; diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md index 6bf13a81c0413..e0fd41a70cd36 100644 --- a/planning/autoware_freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -59,14 +59,15 @@ None | Parameter | Type | Description | | ------------------------- | ------ | -------------------------------------------------- | | `time_limit` | double | time limit of planning | -| `minimum_turning_radius` | double | minimum turning radius of robot | -| `maximum_turning_radius` | double | maximum turning radius of robot | +| `maximum_turning_ratio` | double | max ratio of actual turning range to use | +| `turning_steps` | double | number of turning steps within turning range | | `theta_size` | double | the number of angle's discretization | | `lateral_goal_range` | double | goal range of lateral position | | `longitudinal_goal_range` | double | goal range of longitudinal position | | `angle_goal_range` | double | goal range of angle | | `curve_weight` | double | additional cost factor for curve actions | | `reverse_weight` | double | additional cost factor for reverse actions | +| `direction_change_weight` | double | additional cost factor for switching direction | | `obstacle_threshold` | double | threshold for regarding a certain grid as obstacle | #### A\* search parameters @@ -75,7 +76,9 @@ None | --------------------------- | ------ | ------------------------------------------------------- | | `only_behind_solutions` | bool | whether restricting the solutions to be behind the goal | | `use_back` | bool | whether using backward trajectory | +| `expansion_distance` | double | length of expansion for node transitions | | `distance_heuristic_weight` | double | heuristic weight for estimating node's cost | +| `smoothness_weight` | double | cost factor for change in curvature | #### RRT\* search parameters diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml index 8692aa293cbf7..ee631f8639153 100644 --- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml +++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml @@ -15,16 +15,16 @@ # -- Configurations common to the all planners -- # base configs time_limit: 30000.0 - minimum_turning_radius: 9.0 - maximum_turning_radius: 9.0 - turning_radius_size: 1 + max_turning_ratio: 0.5 # ratio of actual turning limit of vehicle + turning_steps: 1 # search configs theta_size: 144 angle_goal_range: 6.0 - curve_weight: 1.2 - reverse_weight: 2.0 lateral_goal_range: 0.5 longitudinal_goal_range: 2.0 + curve_weight: 0.5 + reverse_weight: 1.0 + direction_change_weight: 1.5 # costmap configs obstacle_threshold: 100 @@ -32,7 +32,9 @@ astar: only_behind_solutions: false use_back: true + expansion_distance: 0.5 distance_heuristic_weight: 1.0 + smoothness_weight: 0.5 # -- RRT* search Configurations -- rrtstar: diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index 55068263f5d5b..d79a06ae52ead 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -245,6 +245,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_shape_.length = vehicle_info.vehicle_length_m; vehicle_shape_.width = vehicle_info.vehicle_width_m; + vehicle_shape_.base_length = vehicle_info.wheel_base_m; + vehicle_shape_.max_steering = vehicle_info.max_steer_angle_rad; vehicle_shape_.base2back = vehicle_info.rear_overhang_m; } @@ -296,18 +298,16 @@ PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() // search configs p.time_limit = declare_parameter("time_limit"); - p.minimum_turning_radius = declare_parameter("minimum_turning_radius"); - p.maximum_turning_radius = declare_parameter("maximum_turning_radius"); - p.turning_radius_size = declare_parameter("turning_radius_size"); - p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); - p.turning_radius_size = std::max(p.turning_radius_size, 1); p.theta_size = declare_parameter("theta_size"); p.angle_goal_range = declare_parameter("angle_goal_range"); p.curve_weight = declare_parameter("curve_weight"); p.reverse_weight = declare_parameter("reverse_weight"); + p.direction_change_weight = declare_parameter("direction_change_weight"); p.lateral_goal_range = declare_parameter("lateral_goal_range"); p.longitudinal_goal_range = declare_parameter("longitudinal_goal_range"); + p.max_turning_ratio = declare_parameter("max_turning_ratio"); + p.turning_steps = declare_parameter("turning_steps"); // costmap configs p.obstacle_threshold = declare_parameter("obstacle_threshold"); @@ -517,7 +517,13 @@ void FreespacePlannerNode::planTrajectory() // execute planning const rclcpp::Time start = get_clock()->now(); - const bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + std::string error_msg; + bool result = false; + try { + result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + } catch (const std::exception & e) { + error_msg = e.what(); + } const rclcpp::Time end = get_clock()->now(); RCLCPP_DEBUG(get_logger(), "Freespace planning: %f [s]", (end - start).seconds()); @@ -530,9 +536,8 @@ void FreespacePlannerNode::planTrajectory() prev_target_index_ = 0; target_index_ = getNextTargetIndex(trajectory_.points.size(), reversing_indices_, prev_target_index_); - } else { - RCLCPP_INFO(get_logger(), "Can't find goal..."); + RCLCPP_INFO(get_logger(), "Can't find goal: %s", error_msg.c_str()); reset(); } } diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp index bca08f0d11c58..4f0a59f44a5d6 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp @@ -23,6 +23,7 @@ #include +#include #include namespace autoware::freespace_planning_algorithms @@ -57,14 +58,21 @@ geometry_msgs::msg::Pose local2global( struct VehicleShape { - double length; // X [m] - double width; // Y [m] + double length; // X [m] + double width; // Y [m] + double base_length; + double max_steering; double base2back; // base_link to rear [m] VehicleShape() = default; - VehicleShape(double length, double width, double base2back) - : length(length), width(width), base2back(base2back) + VehicleShape( + double length, double width, double base_length, double max_steering, double base2back) + : length(length), + width(width), + base_length(base_length), + max_steering(max_steering), + base2back(base2back) { } @@ -72,6 +80,8 @@ struct VehicleShape const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : length(vehicle_info.vehicle_length_m + margin), width(vehicle_info.vehicle_width_m + margin), + base_length(vehicle_info.wheel_base_m), + max_steering(vehicle_info.max_steer_angle_rad), base2back(vehicle_info.rear_overhang_m + margin / 2.0) { } @@ -82,18 +92,16 @@ struct PlannerCommonParam // base configs double time_limit; // planning time limit [msec] - // robot configs - double minimum_turning_radius; // [m] - double maximum_turning_radius; // [m] - int turning_radius_size; // discretized turning radius table size [-] - // search configs int theta_size; // discretized angle table size [-] double curve_weight; // curve moving cost [-] double reverse_weight; // backward moving cost [-] + double direction_change_weight; // direction change cost [-] double lateral_goal_range; // reaching threshold, lateral error [m] double longitudinal_goal_range; // reaching threshold, longitudinal error [m] double angle_goal_range; // reaching threshold, angle error [deg] + double max_turning_ratio; // factor of max turning range to use [-] + int turning_steps; // number of turning steps [-] // costmap configs int obstacle_threshold; // obstacle threshold on grid [-] @@ -120,6 +128,8 @@ class AbstractPlanningAlgorithm const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape) : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape) { + planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1); + collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio; is_collision_table_initialized = false; } @@ -128,6 +138,8 @@ class AbstractPlanningAlgorithm const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin) { + planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1); + collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio; } virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap); @@ -142,8 +154,11 @@ class AbstractPlanningAlgorithm void computeCollisionIndexes( int theta_index, std::vector & indexes, std::vector & vertex_indexes_2d) const; + bool detectBoundaryExit(const IndexXYT & base_index) const; bool detectCollision(const IndexXYT & base_index) const; - inline bool isOutOfRange(const IndexXYT & index) const + + template + inline bool isOutOfRange(const IndexType & index) const { if (index.x < 0 || static_cast(costmap_.info.width) <= index.x) { return true; @@ -153,16 +168,41 @@ class AbstractPlanningAlgorithm } return false; } - inline bool isObs(const IndexXYT & index) const + + template + inline bool isWithinMargin(const IndexType & index) const + { + if ( + index.x < nb_of_margin_cells_ || + static_cast(costmap_.info.width) - index.x < nb_of_margin_cells_) { + return false; + } + if ( + index.y < nb_of_margin_cells_ || + static_cast(costmap_.info.height) - index.y < nb_of_margin_cells_) { + return false; + } + return true; + } + + template + inline bool isObs(const IndexType & index) const { // NOTE: Accessing by .at() instead makes 1.2 times slower here. // Also, boundary check is already done in isOutOfRange before calling this function. // So, basically .at() is not necessary. - return is_obstacle_table_[index.y][index.x]; + return is_obstacle_table_[indexToId(index)]; + } + + // compute single dimensional grid cell index from 2 dimensional index + template + inline int indexToId(const IndexType & index) const + { + return index.y * costmap_.info.width + index.x; } PlannerCommonParam planner_common_param_; - const VehicleShape collision_vehicle_shape_; + VehicleShape collision_vehicle_shape_; // costmap as occupancy grid nav_msgs::msg::OccupancyGrid costmap_; @@ -174,7 +214,7 @@ class AbstractPlanningAlgorithm std::vector> vertex_indexes_table_; // is_obstacle's table - std::vector> is_obstacle_table_; + std::vector is_obstacle_table_; // pose in costmap frame geometry_msgs::msg::Pose start_pose_; @@ -185,6 +225,8 @@ class AbstractPlanningAlgorithm // result path PlannerWaypoints waypoints_; + + int nb_of_margin_cells_; }; } // namespace autoware::freespace_planning_algorithms diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index a746ceea6f838..3f6c3935b8a42 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -34,6 +34,8 @@ namespace autoware::freespace_planning_algorithms { +using geometry_msgs::msg::Pose; + enum class NodeStatus : uint8_t { None, Open, Closed }; struct AstarParam @@ -41,9 +43,11 @@ struct AstarParam // base configs bool only_behind_solutions; // solutions should be behind the goal bool use_back; // backward search + double expansion_distance; // search configs double distance_heuristic_weight; // obstacle threshold on grid [0,255] + double smoothness_weight; }; struct AstarNode @@ -52,20 +56,30 @@ struct AstarNode double x; // x double y; // y double theta; // theta - double gc = 0; // actual cost - double hc = 0; // heuristic cost + double gc = 0; // actual motion cost + double fc = 0; // total node cost + double dir_distance = 0; // distance traveled from last direction change + int steering_index; // steering index bool is_back; // true if the current direction of the vehicle is back AstarNode * parent = nullptr; // parent node - double cost() const { return gc + hc; } + inline void set( + const Pose & pose, const double move_cost, const double total_cost, const double steer_ind, + const bool backward) + { + x = pose.position.x; + y = pose.position.y; + theta = tf2::getYaw(pose.orientation); + gc = move_cost; + fc = total_cost; + steering_index = steer_ind; + is_back = backward; + } }; struct NodeComparison { - bool operator()(const AstarNode * lhs, const AstarNode * rhs) - { - return lhs->cost() > rhs->cost(); - } + bool operator()(const AstarNode * lhs, const AstarNode * rhs) { return lhs->fc > rhs->fc; } }; struct NodeUpdate @@ -74,7 +88,7 @@ struct NodeUpdate double shift_y; double shift_theta; double distance; - bool is_curve; + int steering_index; bool is_back; NodeUpdate rotated(const double theta) const @@ -120,43 +134,45 @@ class AstarSearch : public AbstractPlanningAlgorithm AstarParam{ node.declare_parameter("astar.only_behind_solutions"), node.declare_parameter("astar.use_back"), - node.declare_parameter("astar.distance_heuristic_weight")}) + node.declare_parameter("astar.expansion_distance"), + node.declare_parameter("astar.distance_heuristic_weight"), + node.declare_parameter("astar.smoothness_weight")}) { } - void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override; - bool makePlan( - const geometry_msgs::msg::Pose & start_pose, - const geometry_msgs::msg::Pose & goal_pose) override; + bool makePlan(const Pose & start_pose, const Pose & goal_pose) override; const PlannerWaypoints & getWaypoints() const { return waypoints_; } inline int getKey(const IndexXYT & index) { - return (index.theta + (index.y * x_scale_ + index.x) * y_scale_); + return indexToId(index) * planner_common_param_.theta_size + index.theta; } private: + void setTransitionTable(); + void setCollisionFreeDistanceMap(); bool search(); - void clearNodes(); + void expandNodes(AstarNode & current_node); + void resetData(); void setPath(const AstarNode & goal); bool setStartNode(); bool setGoalNode(); - double estimateCost(const geometry_msgs::msg::Pose & pose) const; + double estimateCost(const Pose & pose, const IndexXYT & index) const; bool isGoal(const AstarNode & node) const; - geometry_msgs::msg::Pose node2pose(const AstarNode & node) const; + Pose node2pose(const AstarNode & node) const; - AstarNode * getNodeRef(const IndexXYT & index) - { - return &(graph_.emplace(getKey(index), AstarNode()).first->second); - } + double getSteeringCost(const int steering_index) const; + double getSteeringChangeCost(const int steering_index, const int prev_steering_index) const; + double getDirectionChangeCost(const double dir_distance) const; // Algorithm specific param AstarParam astar_param_; // hybrid astar variables TransitionTable transition_table_; - std::unordered_map graph_; + std::vector graph_; + std::vector col_free_distance_map_; std::priority_queue, NodeComparison> openlist_; @@ -166,8 +182,9 @@ class AstarSearch : public AbstractPlanningAlgorithm // distance metric option (removed when the reeds_shepp gets stable) bool use_reeds_shepp_; - int x_scale_; - int y_scale_; + double steering_resolution_; + double heading_resolution_; + double avg_turning_radius_; }; } // namespace autoware::freespace_planning_algorithms diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp new file mode 100644 index 0000000000000..0a59646f34d52 --- /dev/null +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ +#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ + +#include +#include + +#include + +#include + +namespace autoware::freespace_planning_algorithms +{ +namespace kinematic_bicycle_model +{ + +static constexpr double eps = 0.001; + +inline double getTurningRadius(const double base_length, const double steering_angle) +{ + return base_length / tan(steering_angle); +} + +inline geometry_msgs::msg::Pose getPoseShift( + const double yaw, const double base_length, const double steering_angle, const double distance) +{ + geometry_msgs::msg::Pose pose; + if (abs(steering_angle) < eps) { + pose.position.x = distance * cos(yaw); + pose.position.y = distance * sin(yaw); + return pose; + } + const double R = getTurningRadius(base_length, steering_angle); + const double beta = distance / R; + pose.position.x = R * sin(yaw + beta) - R * sin(yaw); + pose.position.y = R * cos(yaw) - R * cos(yaw + beta); + pose.orientation = autoware::universe_utils::createQuaternionFromYaw(beta); + return pose; +} + +inline geometry_msgs::msg::Pose getPose( + const geometry_msgs::msg::Pose & current_pose, const double base_length, + const double steering_angle, const double distance) +{ + const double current_yaw = tf2::getYaw(current_pose.orientation); + const auto shift = getPoseShift(current_yaw, base_length, steering_angle, distance); + auto pose = current_pose; + pose.position.x += shift.position.x; + pose.position.y += shift.position.y; + if (abs(steering_angle) > eps) { + pose.orientation = autoware::universe_utils::createQuaternionFromYaw( + current_yaw + tf2::getYaw(shift.orientation)); + } + return pose; +} + +} // namespace kinematic_bicycle_model +} // namespace autoware::freespace_planning_algorithms + +#endif // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_ diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 8932fb277abe8..70de72f2bfffc 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -114,28 +114,30 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) .def_readwrite( "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions) .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back) + .def_readwrite( + "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance) .def_readwrite( "distance_heuristic_weight", - &freespace_planning_algorithms::AstarParam::distance_heuristic_weight); + &freespace_planning_algorithms::AstarParam::distance_heuristic_weight) + .def_readwrite( + "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight); auto pyPlannerCommonParam = py::class_( p, "PlannerCommonParam", py::dynamic_attr()) .def(py::init<>()) .def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit) + .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size) .def_readwrite( - "minimum_turning_radius", - &freespace_planning_algorithms::PlannerCommonParam::minimum_turning_radius) - .def_readwrite( - "maximum_turning_radius", - &freespace_planning_algorithms::PlannerCommonParam::maximum_turning_radius) + "max_turning_ratio", &freespace_planning_algorithms::PlannerCommonParam::max_turning_ratio) .def_readwrite( - "turning_radius_size", - &freespace_planning_algorithms::PlannerCommonParam::turning_radius_size) - .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size) + "turning_steps", &freespace_planning_algorithms::PlannerCommonParam::turning_steps) .def_readwrite( "curve_weight", &freespace_planning_algorithms::PlannerCommonParam::curve_weight) .def_readwrite( "reverse_weight", &freespace_planning_algorithms::PlannerCommonParam::reverse_weight) + .def_readwrite( + "direction_change_weight", + &freespace_planning_algorithms::PlannerCommonParam::direction_change_weight) .def_readwrite( "lateral_goal_range", &freespace_planning_algorithms::PlannerCommonParam::lateral_goal_range) @@ -150,9 +152,11 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) auto pyVehicleShape = py::class_(p, "VehicleShape", py::dynamic_attr()) .def(py::init<>()) - .def(py::init()) + .def(py::init()) .def_readwrite("length", &freespace_planning_algorithms::VehicleShape::length) .def_readwrite("width", &freespace_planning_algorithms::VehicleShape::width) + .def_readwrite("base_length", &freespace_planning_algorithms::VehicleShape::base_length) + .def_readwrite("max_steering", &freespace_planning_algorithms::VehicleShape::max_steering) .def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back); py::class_( diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py index c0cdf8c2ec6ba..93e3acbc345b6 100644 --- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -28,14 +28,14 @@ planner_param = fp.PlannerCommonParam() # base configs planner_param.time_limit = 30000.0 -planner_param.minimum_turning_radius = 9.0 -planner_param.maximum_turning_radius = 9.0 -planner_param.turning_radius_size = 1 +planner_param.max_turning_ratio = 0.5 +planner_param.turning_steps = 1 # search configs planner_param.theta_size = 144 planner_param.angle_goal_range = 6.0 planner_param.curve_weight = 1.2 -planner_param.reverse_weight = 2.0 +planner_param.reverse_weight = 1.0 +planner_param.direction_change_weight = 2.0 planner_param.lateral_goal_range = 0.5 planner_param.longitudinal_goal_range = 2.0 # costmap configs @@ -46,7 +46,9 @@ astar_param = fp.AstarParam() astar_param.only_behind_solutions = False astar_param.use_back = True +astar_param.expansion_distance = 0.4 astar_param.distance_heuristic_weight = 1.0 +astar_param.smoothness_weight = 1.0 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param) diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index c434635ff1401..7ec594d389de3 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,6 +14,8 @@ #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" + #include #include @@ -35,16 +37,16 @@ geometry_msgs::msg::Pose transformPose( int discretizeAngle(const double theta, const int theta_size) { - const double one_angle_range = 2.0 * M_PI / theta_size; - return static_cast(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size; + const double angle_resolution = 2.0 * M_PI / theta_size; + return static_cast(std::round(normalizeRadian(theta, 0.0) / angle_resolution)) % theta_size; } IndexXYT pose2index( const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local, const int theta_size) { - const int index_x = pose_local.position.x / costmap.info.resolution; - const int index_y = pose_local.position.y / costmap.info.resolution; + const int index_x = std::round(pose_local.position.x / costmap.info.resolution); + const int index_y = std::round(pose_local.position.y / costmap.info.resolution); const int index_theta = discretizeAngle(tf2::getYaw(pose_local.orientation), theta_size); return {index_x, index_y, index_theta}; } @@ -57,8 +59,8 @@ geometry_msgs::msg::Pose index2pose( pose_local.position.x = index.x * costmap.info.resolution; pose_local.position.y = index.y * costmap.info.resolution; - const double one_angle_range = 2.0 * M_PI / theta_size; - const double yaw = index.theta * one_angle_range; + const double angle_resolution = 2.0 * M_PI / theta_size; + const double yaw = index.theta * angle_resolution; pose_local.orientation = createQuaternionFromYaw(yaw); return pose_local; @@ -105,20 +107,15 @@ double PlannerWaypoints::compute_length() const void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & costmap) { costmap_ = costmap; - const auto height = costmap_.info.height; - const auto width = costmap_.info.width; + const uint32_t nb_of_cells = costmap_.data.size(); // Initialize status - std::vector> is_obstacle_table; - is_obstacle_table.resize(height); - for (uint32_t i = 0; i < height; i++) { - is_obstacle_table.at(i).resize(width); - for (uint32_t j = 0; j < width; j++) { - const int cost = costmap_.data[i * width + j]; - - if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) { - is_obstacle_table[i][j] = true; - } + std::vector is_obstacle_table; + is_obstacle_table.resize(nb_of_cells); + for (uint32_t i = 0; i < nb_of_cells; ++i) { + const int cost = costmap_.data[i]; + if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) { + is_obstacle_table[i] = true; } } is_obstacle_table_ = is_obstacle_table; @@ -133,6 +130,10 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost } is_collision_table_initialized = true; } + + const double base2front = collision_vehicle_shape_.length - collision_vehicle_shape_.base2back; + nb_of_margin_cells_ = std::ceil( + std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution); } void AbstractPlanningAlgorithm::computeCollisionIndexes( @@ -201,16 +202,12 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes( addIndex2d(back, left, vertex_indexes_2d); } -bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const +bool AbstractPlanningAlgorithm::detectBoundaryExit(const IndexXYT & base_index) const { - if (coll_indexes_table_.empty()) { - std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl; - return false; - } - + if (isWithinMargin(base_index)) return false; const auto & vertex_indexes_2d = vertex_indexes_table_[base_index.theta]; for (const auto & vertex_index_2d : vertex_indexes_2d) { - IndexXYT vertex_index{vertex_index_2d.x, vertex_index_2d.y, 0}; + IndexXY vertex_index{vertex_index_2d.x, vertex_index_2d.y}; // must slide to current base position vertex_index.x += base_index.x; vertex_index.y += base_index.y; @@ -218,11 +215,21 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con return true; } } + return false; +} + +bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const +{ + if (coll_indexes_table_.empty()) { + std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl; + return false; + } + + if (detectBoundaryExit(base_index)) return true; const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { - int idx_theta = 0; // whatever. Yaw is nothing to do with collision detection between grids. - IndexXYT coll_index{coll_index_2d.x, coll_index_2d.y, idx_theta}; + IndexXY coll_index{coll_index_2d.x, coll_index_2d.y}; // must slide to current base position coll_index.x += base_index.x; coll_index.y += base_index.y; diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 1902b7aa2d091..4ead3cb5a7423 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -14,11 +14,15 @@ #include "autoware/freespace_planning_algorithms/astar_search.hpp" +#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" + #include #include #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -29,8 +33,7 @@ namespace autoware::freespace_planning_algorithms { -double calcReedsSheppDistance( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, double radius) +double calcReedsSheppDistance(const Pose & p1, const Pose & p2, double radius) { const auto rs_space = ReedsSheppStateSpace(radius); const ReedsSheppStateSpace::StateXYT pose0{ @@ -45,8 +48,7 @@ void setYaw(geometry_msgs::msg::Quaternion * orientation, const double yaw) *orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); } -geometry_msgs::msg::Pose calcRelativePose( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & pose) +Pose calcRelativePose(const Pose & base_pose, const Pose & pose) { tf2::Transform tf_transform; tf2::convert(base_pose, tf_transform); @@ -62,55 +64,6 @@ geometry_msgs::msg::Pose calcRelativePose( return transformed.pose; } -AstarSearch::TransitionTable createTransitionTable( - const double minimum_turning_radius, const double maximum_turning_radius, - const int turning_radius_size, const double theta_size, const bool use_back) -{ - // Vehicle moving for each angle - AstarSearch::TransitionTable transition_table; - transition_table.resize(theta_size); - - const double dtheta = 2.0 * M_PI / theta_size; - - // Minimum moving distance with one state update - // arc = r * theta - const auto & R_min = minimum_turning_radius; - const auto & R_max = maximum_turning_radius; - const double step_min = R_min * dtheta; - const double dR = (R_max - R_min) / std::max(turning_radius_size - 1, 1); - - // NodeUpdate actions - std::vector forward_node_candidates; - const NodeUpdate forward_straight{step_min, 0.0, 0.0, step_min, false, false}; - forward_node_candidates.push_back(forward_straight); - for (int i = 0; i < turning_radius_size; ++i) { - double R = R_min + i * dR; - double step = R * dtheta; - const double shift_x = R * sin(dtheta); - const double shift_y = R * (1 - cos(dtheta)); - const NodeUpdate forward_left{shift_x, shift_y, dtheta, step, true, false}; - const NodeUpdate forward_right = forward_left.flipped(); - forward_node_candidates.push_back(forward_left); - forward_node_candidates.push_back(forward_right); - } - - for (int i = 0; i < theta_size; i++) { - const double theta = dtheta * i; - - for (const auto & nu : forward_node_candidates) { - transition_table[i].push_back(nu.rotated(theta)); - } - - if (use_back) { - for (const auto & nu : forward_node_candidates) { - transition_table[i].push_back(nu.reversed().rotated(theta)); - } - } - } - - return transition_table; -} - AstarSearch::AstarSearch( const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape, const AstarParam & astar_param) @@ -119,66 +72,134 @@ AstarSearch::AstarSearch( goal_node_(nullptr), use_reeds_shepp_(true) { - transition_table_ = createTransitionTable( - planner_common_param_.minimum_turning_radius, planner_common_param_.maximum_turning_radius, - planner_common_param_.turning_radius_size, planner_common_param_.theta_size, - astar_param_.use_back); + steering_resolution_ = + collision_vehicle_shape_.max_steering / planner_common_param_.turning_steps; + heading_resolution_ = 2.0 * M_PI / planner_common_param_.theta_size; + + const double avg_steering = + steering_resolution_ + (collision_vehicle_shape_.max_steering - steering_resolution_) / 2.0; + avg_turning_radius_ = + kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering); - y_scale_ = planner_common_param.theta_size; + setTransitionTable(); } -void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) +void AstarSearch::setTransitionTable() { - AbstractPlanningAlgorithm::setMap(costmap); + const double distance = astar_param_.expansion_distance; + transition_table_.resize(planner_common_param_.theta_size); + + std::vector forward_transitions; + int steering_ind = -1 * planner_common_param_.turning_steps; + for (; steering_ind <= planner_common_param_.turning_steps; ++steering_ind) { + const double steering = static_cast(steering_ind) * steering_resolution_; + Pose shift_pose = kinematic_bicycle_model::getPoseShift( + 0.0, collision_vehicle_shape_.base_length, steering, distance); + forward_transitions.push_back( + {shift_pose.position.x, shift_pose.position.y, tf2::getYaw(shift_pose.orientation), distance, + steering_ind, false}); + } + + for (int i = 0; i < planner_common_param_.theta_size; ++i) { + const double theta = static_cast(i) * heading_resolution_; + for (const auto & transition : forward_transitions) { + transition_table_[i].push_back(transition.rotated(theta)); + } - x_scale_ = costmap_.info.height; + if (astar_param_.use_back) { + for (const auto & transition : forward_transitions) { + transition_table_[i].push_back(transition.reversed().rotated(theta)); + } + } + } } -bool AstarSearch::makePlan( - const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) +bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose) { + resetData(); + start_pose_ = global2local(costmap_, start_pose); goal_pose_ = global2local(costmap_, goal_pose); - clearNodes(); - graph_.reserve(100000); + if (!setGoalNode()) { + throw std::logic_error("Invalid goal pose"); + } + + setCollisionFreeDistanceMap(); if (!setStartNode()) { - return false; + throw std::logic_error("Invalid start pose"); } - if (!setGoalNode()) { - return false; + if (!search()) { + throw std::logic_error("HA* failed to find path to goal"); } - return search(); + return true; } -void AstarSearch::clearNodes() +void AstarSearch::resetData() { // clearing openlist is necessary because otherwise remaining elements of openlist // point to deleted node. openlist_ = std::priority_queue, NodeComparison>(); + const int nb_of_grid_nodes = costmap_.info.width * costmap_.info.height; + const int total_astar_node_count = nb_of_grid_nodes * planner_common_param_.theta_size; + graph_ = std::vector(total_astar_node_count); + col_free_distance_map_ = + std::vector(nb_of_grid_nodes, std::numeric_limits::max()); +} - graph_ = std::unordered_map(); +void AstarSearch::setCollisionFreeDistanceMap() +{ + using Entry = std::pair; + struct CompareEntry + { + bool operator()(const Entry & a, const Entry & b) { return a.second > b.second; } + }; + std::priority_queue, CompareEntry> heap; + std::vector closed(col_free_distance_map_.size(), false); + auto goal_index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size); + col_free_distance_map_[indexToId(goal_index)] = 0.0; + heap.push({IndexXY{goal_index.x, goal_index.y}, 0.0}); + + Entry current; + std::array offsets = {1, 0, -1}; + while (!heap.empty()) { + current = heap.top(); + heap.pop(); + const int id = indexToId(current.first); + if (closed[id]) continue; + closed[id] = true; + + const auto & index = current.first; + for (const auto & offset_x : offsets) { + const int x = index.x + offset_x; + for (const auto & offset_y : offsets) { + const int y = index.y + offset_y; + const IndexXY n_index{x, y}; + const double offset = std::abs(offset_x) + std::abs(offset_y); + if (isOutOfRange(n_index) || isObs(n_index) || offset < 1) continue; + const int n_id = indexToId(n_index); + const double dist = current.second + (sqrt(offset) * costmap_.info.resolution); + if (closed[n_id] || col_free_distance_map_[n_id] < dist) continue; + col_free_distance_map_[n_id] = dist; + heap.push({n_index, dist}); + } + } + } } bool AstarSearch::setStartNode() { const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size); - if (detectCollision(index)) { - return false; - } + if (detectCollision(index)) return false; // Set start node - AstarNode * start_node = getNodeRef(index); - start_node->x = start_pose_.position.x; - start_node->y = start_pose_.position.y; - start_node->theta = 2.0 * M_PI / planner_common_param_.theta_size * index.theta; - start_node->gc = 0; - start_node->hc = estimateCost(start_pose_); - start_node->is_back = false; + AstarNode * start_node = &graph_[getKey(index)]; + start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false); + start_node->dir_distance = 0.0; start_node->status = NodeStatus::Open; start_node->parent = nullptr; @@ -191,29 +212,18 @@ bool AstarSearch::setStartNode() bool AstarSearch::setGoalNode() { const auto index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size); - - if (detectCollision(index)) { - return false; - } - - return true; + return !detectCollision(index); } -double AstarSearch::estimateCost(const geometry_msgs::msg::Pose & pose) const +double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const { - double total_cost = 0.0; + double total_cost = col_free_distance_map_[indexToId(index)]; // Temporarily, until reeds_shepp gets stable. if (use_reeds_shepp_) { - const double radius = (planner_common_param_.minimum_turning_radius + - planner_common_param_.maximum_turning_radius) * - 0.5; - total_cost += - calcReedsSheppDistance(pose, goal_pose_, radius) * astar_param_.distance_heuristic_weight; - } else { - total_cost += autoware::universe_utils::calcDistance2d(pose, goal_pose_) * - astar_param_.distance_heuristic_weight; + total_cost = + std::max(total_cost, calcReedsSheppDistance(pose, goal_pose_, avg_turning_radius_)); } - return total_cost; + return astar_param_.distance_heuristic_weight * total_cost; } bool AstarSearch::search() @@ -232,6 +242,7 @@ bool AstarSearch::search() // Expand minimum cost node AstarNode * current_node = openlist_.top(); openlist_.pop(); + if (current_node->status == NodeStatus::Closed) continue; current_node->status = NodeStatus::Closed; if (isGoal(*current_node)) { @@ -240,45 +251,81 @@ bool AstarSearch::search() return true; } - // Transit - const auto index_theta = discretizeAngle(current_node->theta, planner_common_param_.theta_size); - for (const auto & transition : transition_table_[index_theta]) { - const bool is_turning_point = transition.is_back != current_node->is_back; + expandNodes(*current_node); + } - const double move_cost = is_turning_point - ? planner_common_param_.reverse_weight * transition.distance - : transition.distance; + // Failed to find path + return false; +} + +void AstarSearch::expandNodes(AstarNode & current_node) +{ + const auto index_theta = discretizeAngle(current_node.theta, planner_common_param_.theta_size); + for (const auto & transition : transition_table_[index_theta]) { + // skip transition back to parent + if ( + current_node.parent != nullptr && transition.is_back != current_node.is_back && + transition.steering_index == current_node.steering_index) { + continue; + } - // Calculate index of the next state - geometry_msgs::msg::Pose next_pose; - next_pose.position.x = current_node->x + transition.shift_x; - next_pose.position.y = current_node->y + transition.shift_y; - setYaw(&next_pose.orientation, current_node->theta + transition.shift_theta); - const auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size); + // Calculate index of the next state + Pose next_pose; + next_pose.position.x = current_node.x + transition.shift_x; + next_pose.position.y = current_node.y + transition.shift_y; + setYaw(&next_pose.orientation, current_node.theta + transition.shift_theta); + const auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size); - if (detectCollision(next_index)) { - continue; - } + if (isOutOfRange(next_index) || isObs(next_index)) continue; - // Compare cost - AstarNode * next_node = getNodeRef(next_index); - if (next_node->status == NodeStatus::None) { - next_node->status = NodeStatus::Open; - next_node->x = next_pose.position.x; - next_node->y = next_pose.position.y; - next_node->theta = tf2::getYaw(next_pose.orientation); - next_node->gc = current_node->gc + move_cost; - next_node->hc = estimateCost(next_pose); - next_node->is_back = transition.is_back; - next_node->parent = current_node; - openlist_.push(next_node); - continue; - } + AstarNode * next_node = &graph_[getKey(next_index)]; + if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue; + + const bool is_direction_switch = + (current_node.parent != nullptr) && (transition.is_back != current_node.is_back); + + double total_weight = 1.0; + total_weight += getSteeringCost(transition.steering_index); + if (transition.is_back) { + total_weight *= (1.0 + planner_common_param_.reverse_weight); + } + + double move_cost = current_node.gc + (total_weight * transition.distance); + move_cost += getSteeringChangeCost(transition.steering_index, current_node.steering_index); + if (is_direction_switch) move_cost += getDirectionChangeCost(current_node.dir_distance); + + double total_cost = move_cost + estimateCost(next_pose, next_index); + // Compare cost + if (next_node->status == NodeStatus::None || next_node->fc > total_cost) { + next_node->status = NodeStatus::Open; + next_node->set( + next_pose, move_cost, total_cost, transition.steering_index, transition.is_back); + next_node->dir_distance = + transition.distance + (is_direction_switch ? 0.0 : current_node.dir_distance); + next_node->parent = ¤t_node; + openlist_.push(next_node); + continue; } } +} - // Failed to find path - return false; +double AstarSearch::getSteeringCost(const int steering_index) const +{ + return planner_common_param_.curve_weight * + (abs(steering_index) / planner_common_param_.turning_steps); +} + +double AstarSearch::getSteeringChangeCost( + const int steering_index, const int prev_steering_index) const +{ + double steering_index_diff = abs(steering_index - prev_steering_index); + return astar_param_.smoothness_weight * steering_index_diff / + (2.0 * planner_common_param_.turning_steps); +} + +double AstarSearch::getDirectionChangeCost(const double dir_distance) const +{ + return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance))); } void AstarSearch::setPath(const AstarNode & goal_node) @@ -293,28 +340,12 @@ void AstarSearch::setPath(const AstarNode & goal_node) // From the goal node to the start node const AstarNode * node = &goal_node; - // push exact goal pose first geometry_msgs::msg::PoseStamped pose; pose.header = header; - pose.pose = local2global(costmap_, goal_pose_); - - PlannerWaypoint pw; - pw.pose = pose; - pw.is_back = node->is_back; - waypoints_.waypoints.push_back(pw); - // push astar nodes poses while (node != nullptr) { - geometry_msgs::msg::PoseStamped pose; - pose.header = header; pose.pose = local2global(costmap_, node2pose(*node)); - - // PlannerWaypoint - PlannerWaypoint pw; - pw.pose = pose; - pw.is_back = node->is_back; - waypoints_.waypoints.push_back(pw); - + waypoints_.waypoints.push_back({pose, node->is_back}); // To the next node node = node->parent; } @@ -357,9 +388,9 @@ bool AstarSearch::isGoal(const AstarNode & node) const return true; } -geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const +Pose AstarSearch::node2pose(const AstarNode & node) const { - geometry_msgs::msg::Pose pose_local; + Pose pose_local; pose_local.position.x = node.x; pose_local.position.y = node.y; diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index 6b0018c517d4a..0ef31959e65fa 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -14,6 +14,8 @@ #include "autoware/freespace_planning_algorithms/rrtstar.hpp" +#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp" + namespace autoware::freespace_planning_algorithms { rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg) @@ -29,6 +31,7 @@ RRTStar::RRTStar( planner_common_param, VehicleShape( original_vehicle_shape.length + 2 * rrtstar_param.margin, original_vehicle_shape.width + 2 * rrtstar_param.margin, + original_vehicle_shape.base_length, original_vehicle_shape.max_steering, original_vehicle_shape.base2back + rrtstar_param.margin)), rrtstar_param_(rrtstar_param), original_vehicle_shape_(original_vehicle_shape) @@ -36,9 +39,6 @@ RRTStar::RRTStar( if (rrtstar_param_.margin <= 0) { throw std::invalid_argument("rrt's collision margin must be greater than 0"); } - if (planner_common_param_.maximum_turning_radius != planner_common_param.minimum_turning_radius) { - throw std::invalid_argument("Currently supports only single radius in rrtstar."); - } } bool RRTStar::makePlan( @@ -50,8 +50,8 @@ bool RRTStar::makePlan( goal_pose_ = global2local(costmap_, goal_pose); const auto is_obstacle_free = [&](const rrtstar_core::Pose & pose) { - const int index_x = pose.x / costmap_.info.resolution; - const int index_y = pose.y / costmap_.info.resolution; + const int index_x = std::round(pose.x / costmap_.info.resolution); + const int index_y = std::round(pose.y / costmap_.info.resolution); const int index_theta = discretizeAngle(pose.yaw, planner_common_param_.theta_size); return !detectCollision(IndexXYT{index_x, index_y, index_theta}); }; @@ -60,7 +60,8 @@ bool RRTStar::makePlan( const rrtstar_core::Pose hi{ costmap_.info.resolution * costmap_.info.width, costmap_.info.resolution * costmap_.info.height, M_PI}; - const double radius = planner_common_param_.minimum_turning_radius; + const double radius = kinematic_bicycle_model::getTurningRadius( + collision_vehicle_shape_.base_length, collision_vehicle_shape_.max_steering); const auto cspace = rrtstar_core::CSpace(lo, hi, radius, is_obstacle_free); const auto x_start = poseMsgToPose(start_pose_); const auto x_goal = poseMsgToPose(goal_pose_); diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp index 7403afd37e295..853d9ef0a9787 100644 --- a/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp @@ -183,17 +183,17 @@ void RRTStar::extend() // This cannot be inside if(is_reached){...} because we must update this anytime after rewiring // takes place double cost_min = inf; - NodeSharedPtr node_best_parent; + NodeSharedPtr reached_node_best_parent; for (const auto & node : reached_nodes_) { const double cost = *(node->cost_from_start) + *(node->cost_to_goal); if (cost < cost_min) { cost_min = cost; - node_best_parent = node; + reached_node_best_parent = node; } } node_goal_->cost_from_start = cost_min; - node_goal_->parent = node_best_parent; - node_goal_->cost_to_parent = node_best_parent->cost_to_goal; + node_goal_->parent = reached_node_best_parent; + node_goal_->cost_to_parent = reached_node_best_parent->cost_to_goal; } } diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 79898ed57327b..31c551e1906db 100644 --- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -37,7 +37,10 @@ namespace fpa = autoware::freespace_planning_algorithms; const double length_lexus = 5.5; const double width_lexus = 2.75; -const fpa::VehicleShape vehicle_shape = fpa::VehicleShape(length_lexus, width_lexus, 1.5); +const double base_length_lexus = 3.0; +const double max_steering_lexus = 0.7; +const fpa::VehicleShape vehicle_shape = + fpa::VehicleShape(length_lexus, width_lexus, base_length_lexus, max_steering_lexus, 1.5); const double pi = 3.1415926; const std::array start_pose{5.5, 4., pi * 0.5}; const std::array goal_pose1{8.0, 26.3, pi * 1.5}; // easiest @@ -170,15 +173,14 @@ fpa::PlannerCommonParam get_default_planner_params() { // set problem configuration const double time_limit = 10 * 1000.0; - const double minimum_turning_radius = 9.0; - const double maximum_turning_radius = 9.0; - const int turning_radius_size = 1; + const double max_turning_ratio = 0.5; + const int turning_steps = 1; const int theta_size = 144; - // Setting weight to 1.0 to fairly compare all algorithms - const double curve_weight = 1.0; + const double curve_weight = 0.5; const double reverse_weight = 1.0; + const double direction_change_weight = 1.5; const double lateral_goal_range = 0.5; const double longitudinal_goal_range = 2.0; @@ -187,15 +189,15 @@ fpa::PlannerCommonParam get_default_planner_params() return fpa::PlannerCommonParam{ time_limit, - minimum_turning_radius, - maximum_turning_radius, - turning_radius_size, theta_size, curve_weight, reverse_weight, + direction_change_weight, lateral_goal_range, longitudinal_goal_range, angle_goal_range, + max_turning_ratio, + turning_steps, obstacle_threshold}; } @@ -203,16 +205,18 @@ std::unique_ptr configure_astar(bool use_multi) { auto planner_common_param = get_default_planner_params(); if (use_multi) { - planner_common_param.maximum_turning_radius = 14.0; - planner_common_param.turning_radius_size = 3; + planner_common_param.turning_steps = 3; } // configure astar param const bool only_behind_solutions = false; const bool use_back = true; - const double distance_heuristic_weight = 1.0; - const auto astar_param = - fpa::AstarParam{only_behind_solutions, use_back, distance_heuristic_weight}; + const double expansion_distance = 0.4; + const double distance_heuristic_weight = 2.0; + const double smoothness_weight = 0.5; + const auto astar_param = fpa::AstarParam{ + only_behind_solutions, use_back, expansion_distance, distance_heuristic_weight, + smoothness_weight}; auto algo = std::make_unique(planner_common_param, vehicle_shape, astar_param); return algo; diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp index 25664b14320c1..586866f9a94d6 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp @@ -46,14 +46,14 @@ class AdaptiveCruiseController const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, - const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); void insertAdaptiveCruiseVelocity( const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, + const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header, const PredictedObject & target_object); diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp index a54d60835aadf..3cacdc2e1ecaf 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp @@ -158,13 +158,13 @@ size_t findEgoSegmentIndex( } std::vector resampleTrajectoryPoints( - const std::vector traj_points, const double interval); + const std::vector & traj_points, const double interval); std::vector resampleTrajectoryPointsWithoutStopPoint( - const std::vector traj_points, const double interval); + const std::vector & traj_points, const double interval); std::vector resampleReferencePoints( - const std::vector ref_points, const double interval); + const std::vector & ref_points, const double interval); template std::optional updateFrontPointForFix( diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 433d1d0995088..8e15cdb852893 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -136,7 +136,7 @@ geometry_msgs::msg::Point getNearestPosition( } std::vector resampleTrajectoryPoints( - const std::vector traj_points, const double interval) + const std::vector & traj_points, const double interval) { constexpr bool enable_resampling_stop_point = true; @@ -148,7 +148,7 @@ std::vector resampleTrajectoryPoints( // NOTE: stop point will not be resampled std::vector resampleTrajectoryPointsWithoutStopPoint( - const std::vector traj_points, const double interval) + const std::vector & traj_points, const double interval) { constexpr bool enable_resampling_stop_point = false; @@ -159,7 +159,7 @@ std::vector resampleTrajectoryPointsWithoutStopPoint( } std::vector resampleReferencePoints( - const std::vector ref_points, const double interval) + const std::vector & ref_points, const double interval) { // resample pose and velocity const auto traj_points = convertToTrajectoryPoints(ref_points); diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp index 77a9dbae81a0c..f989ab76f6952 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/utils/trajectory_utils.hpp @@ -116,10 +116,10 @@ size_t findEgoSegmentIndex( Path create_path(Path path_msg, const std::vector & traj_points); std::vector resampleTrajectoryPoints( - const std::vector traj_points, const double interval); + const std::vector & traj_points, const double interval); std::vector resampleTrajectoryPointsWithoutStopPoint( - const std::vector traj_points, const double interval); + const std::vector & traj_points, const double interval); template std::optional updateFrontPointForFix( diff --git a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index 4a36359ded4f3..e533b963cf655 100644 --- a/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -50,7 +50,7 @@ Path create_path(Path path_msg, const std::vector & traj_points } std::vector resampleTrajectoryPoints( - const std::vector traj_points, const double interval) + const std::vector & traj_points, const double interval) { constexpr bool enable_resampling_stop_point = true; @@ -62,7 +62,7 @@ std::vector resampleTrajectoryPoints( // NOTE: stop point will not be resampled std::vector resampleTrajectoryPointsWithoutStopPoint( - const std::vector traj_points, const double interval) + const std::vector & traj_points, const double interval) { constexpr bool enable_resampling_stop_point = false; 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..705395c2b1741 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,14 @@ class RTCInterface void removeExpiredCooperateStatus(); void clearCooperateStatus(); bool isActivated(const UUID & uuid) const; + bool isForceActivated(const UUID & uuid) const; + bool isForceDeactivated(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..2808a9cc3b8cc 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; @@ -151,21 +179,21 @@ std::vector RTCInterface::validateCooperateCommands( const auto itr = std::find_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), - [command](auto & s) { return s.uuid == command.uuid; }); + [command](const auto & s) { return s.uuid == command.uuid; }); if (itr != registered_status_.statuses.end()) { if (itr->state.type == State::WAITING_FOR_EXECUTION || itr->state.type == State::RUNNING) { response.success = true; } 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; } @@ -180,7 +208,7 @@ void RTCInterface::updateCooperateCommandStatus(const std::vector lock(mutex_); const auto itr = std::find_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), - [uuid](auto & s) { return s.uuid == uuid; }); + [uuid](const auto & s) { return s.uuid == uuid; }); if (itr != registered_status_.statuses.end()) { if (itr->state.type == State::FAILED || itr->state.type == State::SUCCEEDED) { @@ -313,7 +342,55 @@ 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; +} + +bool RTCInterface::isForceDeactivated(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::RUNNING) { + return false; + } + if (itr->command_status.type == Command::DEACTIVATE && itr->safe && !itr->auto_mode) { + return true; + } else { + return false; + } + } + + RCLCPP_WARN_STREAM( + getLogger(), + "[isForceDeactivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl); return false; } @@ -322,7 +399,7 @@ bool RTCInterface::isRegistered(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; }); + [uuid](const auto & s) { return s.uuid == uuid; }); return itr != registered_status_.statuses.end(); } @@ -331,14 +408,30 @@ bool RTCInterface::isRTCEnabled(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; }); + [uuid](const auto & s) { return s.uuid == uuid; }); if (itr != registered_status_.statuses.end()) { return !itr->auto_mode; } 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](const 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 +456,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_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 0e204ae82146c..54496a66910b0 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -37,11 +37,9 @@ std::shared_ptr findNearestParkinglot( const std::shared_ptr & lanelet_map_ptr, const lanelet::BasicPoint2d & current_position) { - const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); - const auto linked_parking_lot = std::make_shared(); const auto result = lanelet::utils::query::getLinkedParkingLot( - current_position, all_parking_lots, linked_parking_lot.get()); + current_position, lanelet_map_ptr, linked_parking_lot.get()); if (result) { return linked_parking_lot; 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..a3e7088d1e88f 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -69,7 +69,7 @@ std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) } diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose) + const std::string & no_start_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; @@ -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 4e142ca8983f7..d6033b66d5d6f 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -95,7 +95,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"}; 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 bbc3828bb1b15..bfcd7db6b9c43 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 @@ -73,7 +73,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, 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 06a6f2da7f30a..79dd5cda20d4b 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 @@ -19,7 +19,7 @@ #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" +#include "qp_interface/qp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -47,7 +47,8 @@ class JerkFilteredSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, @@ -59,7 +60,7 @@ class JerkFilteredSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + std::shared_ptr qp_interface_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")}; TrajectoryPoints forwardJerkFilter( 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 a2a07ec6909aa..8f6bbc2236eb6 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 @@ -45,7 +45,8 @@ class L2PseudoJerkSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, 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 7c46a53431608..e27a4db10e748 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 @@ -45,7 +45,8 @@ class LinfPseudoJerkSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, 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 6671eaa3eabe7..7206a64ea32eb 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 @@ -61,7 +61,8 @@ class SmootherBase virtual ~SmootherBase() = default; virtual bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) = 0; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) = 0; virtual TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 997f7b16e5652..f5cd3f2667c04 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -9,6 +9,7 @@ Takamasa Horibe Makoto Kurihara Satoshi Ota + Go Sakayori Apache License 2.0 Takamasa Horibe @@ -29,6 +30,7 @@ interpolation nav_msgs osqp_interface + qp_interface rclcpp tf2 tf2_ros diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 195288dd9c896..a358a1b61c11b 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -433,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; @@ -629,7 +629,8 @@ bool VelocitySmootherNode::smoothVelocity( std::vector debug_trajectories; if (!smoother_->apply( - initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) { + initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories, + publish_debug_trajs_)) { RCLCPP_WARN(get_logger(), "Fail to solve optimization."); } @@ -669,15 +670,13 @@ bool VelocitySmootherNode::smoothVelocity( pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); } - if (!debug_trajectories.empty()) { - for (auto & debug_trajectory : debug_trajectories) { - debug_trajectory.insert( - debug_trajectory.begin(), traj_resampled.begin(), - traj_resampled.begin() + traj_resampled_closest); - for (size_t i = 0; i < traj_resampled_closest; ++i) { - debug_trajectory.at(i).longitudinal_velocity_mps = - debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; - } + for (auto & debug_trajectory : debug_trajectories) { + debug_trajectory.insert( + debug_trajectory.begin(), traj_resampled.begin(), + traj_resampled.begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { + debug_trajectory.at(i).longitudinal_velocity_mps = + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; } } publishDebugTrajectories(debug_trajectories); @@ -1079,7 +1078,7 @@ bool VelocitySmootherNode::isReverse(const TrajectoryPoints & points) const if (points.empty()) return true; return std::any_of( - points.begin(), points.end(), [](auto & pt) { return pt.longitudinal_velocity_mps < 0; }); + points.begin(), points.end(), [](const auto & pt) { return pt.longitudinal_velocity_mps < 0; }); } void VelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const { 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 3a6bc736c96ab..fae1b3590cacd 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 @@ -103,7 +103,8 @@ AnalyticalJerkConstrainedSmoother::Param AnalyticalJerkConstrainedSmoother::getP bool AnalyticalJerkConstrainedSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories) + TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { RCLCPP_DEBUG(logger_, "-------------------- Start --------------------"); 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 d458c688d060c..3e363b8bcbab9 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -15,6 +15,7 @@ #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" +#include "qp_interface/proxqp_interface.hpp" #include @@ -40,11 +41,8 @@ JerkFilteredSmoother::JerkFilteredSmoother( p.over_j_weight = node.declare_parameter("over_j_weight"); p.jerk_filter_ds = node.declare_parameter("jerk_filter_ds"); - qp_solver_.updateMaxIter(20000); - qp_solver_.updateRhoInterval(0); // 0 means automatic - qp_solver_.updateEpsRel(1.0e-6); // def: 1.0e-4 - qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 - qp_solver_.updateVerbose(false); + qp_interface_ = + std::make_shared(false, 20000, 1.0e-8, 1.0e-6, false); } void JerkFilteredSmoother::setParam(const Param & smoother_param) @@ -59,7 +57,7 @@ JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, - std::vector & debug_trajectories) + std::vector & debug_trajectories, const bool publish_debug_trajs) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -116,10 +114,12 @@ bool JerkFilteredSmoother::apply( auto opt_resampled_trajectory = resample(filtered); // Set debug trajectories - debug_trajectories.resize(3); - debug_trajectories[0] = resample(forward_filtered); - debug_trajectories[1] = resample(backward_filtered); - debug_trajectories[2] = resample(filtered); + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = resample(forward_filtered); + debug_trajectories[1] = resample(backward_filtered); + debug_trajectories[2] = resample(filtered); + } // Ensure terminal velocity is zero opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0; @@ -129,9 +129,12 @@ bool JerkFilteredSmoother::apply( // No need to do optimization output.front().longitudinal_velocity_mps = v0; output.front().acceleration_mps2 = a0; - debug_trajectories[0] = output; - debug_trajectories[1] = output; - debug_trajectories[2] = output; + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = output; + debug_trajectories[1] = output; + debug_trajectories[2] = output; + } return true; } @@ -301,14 +304,13 @@ bool JerkFilteredSmoother::apply( // execute optimization time_keeper_->start_track("optimize"); - const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + const auto optval = qp_interface_->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) { - RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + if (!qp_interface_->isSolved()) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_interface_->getStatus().c_str()); return false; } + const auto has_nan = std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); if (has_nan) { @@ -332,16 +334,6 @@ bool JerkFilteredSmoother::apply( output.at(i).acceleration_mps2 = a_stop_decel; } - qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]"); - - const int status_polish = std::get<2>(result); - if (status_polish != 1) { - const auto msg = status_polish == 0 ? "unperformed" - : status_polish == -1 ? "unsuccessful" - : "unknown"; - RCLCPP_DEBUG(logger_, "osqp polish process failed : %s. The result may be inaccurate", msg); - } - if (VERBOSE_TRAJECTORY_VELOCITY) { const auto s_output = trajectory_utils::calcArclengthArray(output); 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 f379b217187c9..678c8e01bf67e 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 @@ -53,7 +53,8 @@ L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const bool L2PseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { debug_trajectories.clear(); 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 2ab3d6dd80dfc..60345ff0fa6f4 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 @@ -53,7 +53,8 @@ LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const bool LinfPseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { debug_trajectories.clear(); diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index ea0453370e03a..830b34589a069 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -229,7 +229,7 @@ void setZeroVelocity(TrajectoryPoints & trajectory) double getMaxVelocity(const TrajectoryPoints & trajectory) { double max_vel = 0.0; - for (auto & tp : trajectory) { + for (const auto & tp : trajectory) { if (tp.longitudinal_velocity_mps > max_vel) { max_vel = tp.longitudinal_velocity_mps; } @@ -240,7 +240,7 @@ double getMaxVelocity(const TrajectoryPoints & trajectory) double getMaxAbsVelocity(const TrajectoryPoints & trajectory) { double max_vel = 0.0; - for (auto & tp : trajectory) { + for (const auto & tp : trajectory) { double abs_vel = std::fabs(tp.longitudinal_velocity_mps); if (abs_vel > max_vel) { max_vel = abs_vel; @@ -434,7 +434,7 @@ std::optional applyDecelFilterWithJerkConstraint( const std::map & jerk_profile) { double t_total{0.0}; - for (auto & it : jerk_profile) { + for (const auto & it : jerk_profile) { t_total += it.second; } @@ -544,7 +544,7 @@ std::optional> updateStateWithJerkCon double x{0.0}; double t_sum{0.0}; - for (auto & it : jerk_profile) { + for (const auto & it : jerk_profile) { j = it.first; t_sum += it.second; if (t > t_sum) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index e6f84e7eece0c..61908b524c253 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -90,9 +90,8 @@ velocity: 1.0 vehicle_shape_margin: 1.0 time_limit: 3000.0 - minimum_turning_radius: 5.0 - maximum_turning_radius: 5.0 - turning_radius_size: 1 + max_turning_ratio: 0.7 + turning_steps: 1 # search configs search_configs: theta_size: 144 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index e34cd202011d0..749ed57f95cc9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -228,6 +228,8 @@ class ThreadSafeData DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data) DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) private: std::shared_ptr pull_over_path_{nullptr}; @@ -241,6 +243,8 @@ class ThreadSafeData std::optional last_previous_module_output_{}; PreviousPullOverData prev_data_{}; CollisionCheckDebugMap collision_check_{}; + PredictedObjects static_target_objects_{}; + PredictedObjects dynamic_target_objects_{}; std::recursive_mutex & mutex_; rclcpp::Clock::SharedPtr clock_; @@ -520,9 +524,10 @@ class GoalPlannerModule : public SceneModuleInterface const PathWithLaneId & path, const std::shared_ptr occupancy_grid_map) const; bool checkObjectsCollision( - const PathWithLaneId & path, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const double collision_check_margin, - const bool extract_static_objects, const bool update_debug_data = false) const; + const PathWithLaneId & path, const std::vector & curvatures, + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const double collision_check_margin, const bool extract_static_objects, + const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp index 05385600926f5..9899f4a5fad1e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -52,41 +52,29 @@ struct PullOverPath size_t goal_id{}; size_t id{}; bool decided_velocity{false}; - double max_curvature{0.0}; // max curvature of the parking path - PathWithLaneId getFullPath() const + /** + * @brief Set paths and start/end poses + * By setting partial_paths, full_path, parking_path and curvature are also set at the same time + * @param input_partial_paths partial paths + * @param input_start_pose start pose + * @param input_end_pose end pose + */ + void setPaths( + const std::vector input_partial_paths, const Pose & input_start_pose, + const Pose & input_end_pose) { - PathWithLaneId path{}; - for (size_t i = 0; i < partial_paths.size(); ++i) { - if (i == 0) { - path.points.insert( - path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); - } else { - // skip overlapping point - path.points.insert( - path.points.end(), next(partial_paths.at(i).points.begin()), - partial_paths.at(i).points.end()); - } - } - path.points = autoware::motion_utils::removeOverlapPoints(path.points); + partial_paths = input_partial_paths; + start_pose = input_start_pose; + end_pose = input_end_pose; - return path; + updatePathData(); } - // path from the pull over start pose to the end pose - PathWithLaneId getParkingPath() const - { - const PathWithLaneId full_path = getFullPath(); - const size_t start_idx = - autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); + // Note: return copy value (not const&) because the value is used in multi threads + PathWithLaneId getFullPath() const { return full_path; } - PathWithLaneId parking_path{}; - std::copy( - full_path.points.begin() + start_idx, full_path.points.end(), - std::back_inserter(parking_path.points)); - - return parking_path; - } + PathWithLaneId getParkingPath() const { return parking_path; } PathWithLaneId getCurrentPath() const { @@ -108,6 +96,82 @@ struct PullOverPath } bool isValidPath() const { return type != PullOverPlannerType::NONE; } + + std::vector getFullPathCurvatures() const { return full_path_curvatures; } + std::vector getParkingPathCurvatures() const { return parking_path_curvatures; } + double getFullPathMaxCurvature() const { return full_path_max_curvature; } + double getParkingPathMaxCurvature() const { return parking_path_max_curvature; } + +private: + void updatePathData() + { + updateFullPath(); + updateParkingPath(); + updateCurvatures(); + } + + void updateFullPath() + { + PathWithLaneId path{}; + for (size_t i = 0; i < partial_paths.size(); ++i) { + if (i == 0) { + path.points.insert( + path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(partial_paths.at(i).points.begin()), + partial_paths.at(i).points.end()); + } + } + full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + } + + void updateParkingPath() + { + if (full_path.points.empty()) { + updateFullPath(); + } + const size_t start_idx = + autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); + + PathWithLaneId path{}; + std::copy( + full_path.points.begin() + start_idx, full_path.points.end(), + std::back_inserter(path.points)); + parking_path = path; + } + + void updateCurvatures() + { + const auto calculateCurvaturesAndMax = + [](const auto & path) -> std::pair, double> { + std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); + double max_curvature = 0.0; + if (!curvatures.empty()) { + max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + } + return std::make_pair(curvatures, max_curvature); + }; + std::tie(full_path_curvatures, full_path_max_curvature) = + calculateCurvaturesAndMax(getFullPath()); + std::tie(parking_path_curvatures, parking_path_max_curvature) = + calculateCurvaturesAndMax(getParkingPath()); + } + + // curvatures + std::vector full_path_curvatures{}; + std::vector parking_path_curvatures{}; + std::vector current_path_curvatures{}; + double parking_path_max_curvature{0.0}; + double full_path_max_curvature{0.0}; + double current_path_max_curvature{0.0}; + + // path + PathWithLaneId full_path{}; + PathWithLaneId parking_path{}; }; class PullOverPlannerBase diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 98a513d519b43..3c717a90c93ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -42,6 +42,7 @@ using geometry_msgs::msg::Twist; using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using Shape = autoware_perception_msgs::msg::Shape; using Polygon2d = autoware::universe_utils::Polygon2d; lanelet::ConstLanelets getPullOverLanes( 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 1f79f330be676..57c35fc4dd8ba 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 @@ -58,7 +58,12 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) const Pose end_pose = use_back_ ? goal_pose : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); - if (!planner_->makePlan(current_pose, end_pose)) { + + try { + if (!planner_->makePlan(current_pose, end_pose)) { + return {}; + } + } catch (const std::exception & e) { return {}; } @@ -129,10 +134,8 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) } PullOverPath pull_over_path{}; - pull_over_path.partial_paths = partial_paths; pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; - pull_over_path.start_pose = current_pose; - pull_over_path.end_pose = goal_pose; + pull_over_path.setPaths(partial_paths, current_pose, goal_pose); pull_over_path.type = getPlannerType(); return pull_over_path; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp index ffae76bff1abe..3baa603f4d00c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -72,10 +72,8 @@ std::optional GeometricPullOver::plan(const Pose & goal_pose) PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); - pull_over_path.partial_paths = planner_.getPaths(); pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); - pull_over_path.start_pose = planner_.getStartPose(); - pull_over_path.end_pose = planner_.getArcEndPose(); + pull_over_path.setPaths(planner_.getPaths(), planner_.getStartPose(), planner_.getArcEndPose()); return pull_over_path; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 1a711adb133eb..419f3bd2dda47 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -123,6 +123,13 @@ GoalPlannerModule::GoalPlannerModule( if (parameters_->safety_check_params.enable_safety_check) { initializeSafetyCheckParameters(); } + + /** + * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called + * from the main thread only. + * If you want to see the processing time tree in console, uncomment the following line + */ + // time_keeper_->add_reporter(&std::cerr); } bool GoalPlannerModule::hasPreviousModulePathShapeChanged( @@ -299,12 +306,6 @@ void GoalPlannerModule::onTimer() // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority - const std::vector curvatures = - autoware::motion_utils::calcCurvature(pull_over_path->getParkingPath().points); - pull_over_path->max_curvature = std::abs(*std::max_element( - curvatures.begin(), curvatures.end(), - [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); - path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -447,6 +448,24 @@ void GoalPlannerModule::updateObjectsFilteringParams( void GoalPlannerModule::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // extract static and dynamic objects in expanded pull over lanes + { + const auto & p = parameters_; + const auto & rh = *(planner_data_->route_handler); + const auto objects = *(planner_data_->dynamic_object); + const auto static_target_objects = + goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects, p->th_moving_object_velocity); + const auto dynamic_target_objects = goal_planner_utils::extractObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects); + thread_safe_data_.set_static_target_objects(static_target_objects); + thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects); + } + // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then @@ -755,16 +774,17 @@ bool GoalPlannerModule::canReturnToLaneParking() return false; } - if (!thread_safe_data_.get_lane_parking_pull_over_path()) { + const auto lane_parking_path = thread_safe_data_.get_lane_parking_pull_over_path(); + if (!lane_parking_path) { return false; } - const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath(); - + const PathWithLaneId path = lane_parking_path->getFullPath(); + const std::vector curvatures = lane_parking_path->getFullPathCurvatures(); if ( parameters_->use_object_recognition && checkObjectsCollision( - path, planner_data_, *parameters_, + path, curvatures, planner_data_, *parameters_, parameters_->object_recognition_collision_check_hard_margins.back(), /*extract_static_objects=*/false)) { return false; @@ -789,6 +809,8 @@ bool GoalPlannerModule::canReturnToLaneParking() GoalCandidates GoalPlannerModule::generateGoalCandidates() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // calculate goal candidates const auto & route_handler = planner_data_->route_handler; if (utils::isAllowedGoalModification(route_handler)) { @@ -809,6 +831,8 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const BehaviorModuleOutput GoalPlannerModule::plan() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(); } @@ -821,6 +845,8 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // ========================================================================================== // print path priority for debug const auto debugPrintPathPriority = @@ -855,7 +881,8 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri << ", path_id: " << path.id << ", goal_id: " << path.goal_id << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") << ", margin: " << path_id_to_margin_map.at(path.id) - << (isSoftMargin(path) ? " (soft)" : " (hard)") << ", curvature: " << path.max_curvature + << (isSoftMargin(path) ? " (soft)" : " (hard)") + << ", curvature: " << path.getParkingPathMaxCurvature() << (isHighCurvature(path) ? " (high)" : " (low)"); ss << "\n"; } @@ -911,11 +938,11 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return 0.0; } // check path collision margin - const auto resampled_path = - utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); + const PathWithLaneId parking_path = pull_over_path.getParkingPath(); + const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); for (const auto & margin : margins) { if (!checkObjectsCollision( - resampled_path, planner_data_, *parameters_, margin, + parking_path, parking_path_curvatures, planner_data_, *parameters_, margin, /*extract_static_objects=*/true)) { return margin; } @@ -942,7 +969,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri // (3) Sort by curvature // If the curvature is less than the threshold, prioritize the path. const auto isHighCurvature = [&](const PullOverPath & path) -> bool { - return path.max_curvature >= parameters_->high_curvature_threshold; + return path.getParkingPathMaxCurvature() >= parameters_->high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { @@ -1013,6 +1040,8 @@ std::optional> GoalPlannerModule::selectP const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const double collision_check_margin) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); const double backward_length = parameters_->backward_goal_search_length + parameters_->decide_path_distance; @@ -1048,11 +1077,14 @@ std::optional> GoalPlannerModule::selectP continue; } - const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); + // const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), + // 0.5); + const PathWithLaneId parking_path = pull_over_path.getParkingPath(); + const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); if ( parameters_->use_object_recognition && checkObjectsCollision( - resampled_path, planner_data_, *parameters_, collision_check_margin, + parking_path, parking_path_curvatures, planner_data_, *parameters_, collision_check_margin, /*extract_static_objects=*/true, /*update_debug_data=*/true)) { continue; @@ -1060,7 +1092,7 @@ std::optional> GoalPlannerModule::selectP if ( parameters_->use_occupancy_grid_for_path_collision_check && - checkOccupancyGridCollision(resampled_path, occupancy_grid_map_)) { + checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } @@ -1072,6 +1104,8 @@ std::optional> GoalPlannerModule::selectP std::vector GoalPlannerModule::generateDrivableLanes() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, @@ -1084,6 +1118,8 @@ std::vector GoalPlannerModule::generateDrivableLanes() const void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + output.reference_path = getPreviousModuleOutput().reference_path; if (!thread_safe_data_.foundPullOverPath()) { @@ -1135,6 +1171,8 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = @@ -1201,6 +1239,8 @@ bool GoalPlannerModule::hasDecidedPath( const std::shared_ptr & safety_check_params, const std::shared_ptr goal_searcher) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + return checkDecidingPathStatus( planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, goal_searcher) @@ -1216,6 +1256,8 @@ bool GoalPlannerModule::hasNotDecidedPath( const std::shared_ptr & safety_check_params, const std::shared_ptr goal_searcher) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + return checkDecidingPathStatus( planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params, goal_searcher) @@ -1231,6 +1273,8 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( const std::shared_ptr & safety_check_params, const std::shared_ptr goal_searcher) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status; const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; @@ -1278,11 +1322,13 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( } // check current parking path collision - const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5); + const auto parking_path = pull_over_path->getParkingPath(); + const std::vector parking_path_curvatures = pull_over_path->getParkingPathCurvatures(); const double margin = parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; if (checkObjectsCollision( - parking_path, planner_data, parameters, margin, /*extract_static_objects=*/false)) { + parking_path, parking_path_curvatures, planner_data, parameters, margin, + /*extract_static_objects=*/false)) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); @@ -1345,6 +1391,8 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( void GoalPlannerModule::decideVelocity() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); @@ -1357,6 +1405,8 @@ void GoalPlannerModule::decideVelocity() BehaviorModuleOutput GoalPlannerModule::planPullOver() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!hasDecidedPath( planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, safety_check_params_, goal_searcher_)) { @@ -1368,6 +1418,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver() BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // if pull over path candidates generation is not finished, use previous module output if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); @@ -1398,6 +1450,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // if pull over path candidates generation is not finished, use previous module output if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); @@ -1475,6 +1529,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() void GoalPlannerModule::postProcess() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!thread_safe_data_.foundPullOverPath()) { return; } @@ -1502,6 +1558,8 @@ void GoalPlannerModule::postProcess() void GoalPlannerModule::updatePreviousData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. // TODO(Mamoru Sobue): put prev_data_ out of ThreadSafeData @@ -1537,6 +1595,8 @@ void GoalPlannerModule::updatePreviousData() BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(); } @@ -1547,6 +1607,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() std::pair GoalPlannerModule::calcDistanceToPathChange() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!thread_safe_data_.foundPullOverPath()) { return {std::numeric_limits::max(), std::numeric_limits::max()}; } @@ -1581,6 +1643,8 @@ void GoalPlannerModule::setParameters(const std::shared_ptrroute_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; @@ -1688,6 +1752,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); @@ -1763,13 +1829,16 @@ bool GoalPlannerModule::isStuck( return false; } - if ( - parameters.use_object_recognition && - checkObjectsCollision( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), planner_data, parameters, - /*extract_static_objects=*/false, - parameters.object_recognition_collision_check_hard_margins.back())) { - return true; + const auto pull_over_path = thread_safe_data_.get_pull_over_path(); + if (parameters.use_object_recognition) { + const auto path = pull_over_path->getCurrentPath(); + const auto curvatures = autoware::motion_utils::calcCurvature(path.points); + if (checkObjectsCollision( + path, curvatures, planner_data, parameters, + parameters.object_recognition_collision_check_hard_margins.back(), + /*extract_static_objects=*/false)) { + return true; + } } if ( @@ -1784,6 +1853,8 @@ bool GoalPlannerModule::isStuck( bool GoalPlannerModule::hasFinishedCurrentPath() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!last_approval_data_) { return false; } @@ -1834,6 +1905,8 @@ bool GoalPlannerModule::isOnModifiedGoal( TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto path = thread_safe_data_.get_pull_over_path()->getFullPath(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; @@ -1913,23 +1986,29 @@ bool GoalPlannerModule::checkOccupancyGridCollision( } bool GoalPlannerModule::checkObjectsCollision( - const PathWithLaneId & path, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const double collision_check_margin, - const bool extract_static_objects, const bool update_debug_data) const -{ - const auto target_objects = std::invoke([&]() { - const auto & p = parameters; - const auto & rh = *(planner_data->route_handler); - const auto objects = *(planner_data->dynamic_object); - if (extract_static_objects) { - return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( - rh, left_side_parking_, p.backward_goal_search_length, p.forward_goal_search_length, - p.detection_bound_offset, objects, p.th_moving_object_velocity); - } - return goal_planner_utils::extractObjectsInExpandedPullOverLanes( - rh, left_side_parking_, p.backward_goal_search_length, p.forward_goal_search_length, - p.detection_bound_offset, objects); - }); + const PathWithLaneId & path, const std::vector & curvatures, + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const double collision_check_margin, const bool extract_static_objects, + const bool update_debug_data) const +{ + const auto target_objects = extract_static_objects + ? thread_safe_data_.get_static_target_objects() + : thread_safe_data_.get_dynamic_target_objects(); + if (target_objects.objects.empty()) { + return false; + } + + // check collision roughly with {min_distance, max_distance} between ego footprint and objects + // footprint + std::pair has_collision_rough = + utils::path_safety_checker::checkObjectsCollisionRough( + path, target_objects, collision_check_margin, planner_data_->parameters, false); + if (!has_collision_rough.first) { + return false; + } + if (has_collision_rough.second) { + return true; + } std::vector obj_polygons; for (const auto & object : target_objects.objects) { @@ -1941,18 +2020,22 @@ bool GoalPlannerModule::checkObjectsCollision( * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. * - `extra_lateral_margin` adds the lateral margin on curves. */ + if (path.points.size() != curvatures.size()) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "path.points.size() != curvatures.size() in checkObjectsCollision(). judge as non collision"); + return false; + } std::vector ego_polygons_expanded{}; - const auto curvatures = autoware::motion_utils::calcCurvature(path.points); for (size_t i = 0; i < path.points.size(); ++i) { const auto p = path.points.at(i); - const double extra_stopping_margin = std::min( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin); // The square is meant to imply centrifugal force, but it is not a very well-founded formula. - // TODO(kosuke55): It is needed to consider better way because there is an inherently different - // conception of the inside and outside margins. + // TODO(kosuke55): It is needed to consider better way because there is an inherently + // different conception of the inside and outside margins. const double extra_lateral_margin = std::min( extra_stopping_margin, std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); @@ -2072,6 +2155,8 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const double time = planner_data_->parameters.turn_signal_search_time; const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -2111,6 +2196,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const double pull_over_velocity = parameters_->pull_over_velocity; const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -2130,6 +2217,8 @@ void GoalPlannerModule::decelerateBeforeSearchStart( bool GoalPlannerModule::isCrossingPossible( const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (start_lane.centerline().empty() || end_lane.centerline().empty()) { return false; } @@ -2279,6 +2368,8 @@ std::pair GoalPlannerModule::isSafePath( const std::shared_ptr & objects_filtering_params, const std::shared_ptr & safety_check_params) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!thread_safe_data_.get_pull_over_path()) { return {false, false}; } @@ -2415,6 +2506,8 @@ std::pair GoalPlannerModule::isSafePath( void GoalPlannerModule::setDebugData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + debug_marker_.markers.clear(); using autoware::motion_utils::createStopVirtualWallMarker; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index b86bbbca7d98b..8a4f3ab202a67 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -201,17 +201,10 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); p.freespace_parking_common_parameters.time_limit = node->declare_parameter(ns + "time_limit"); - p.freespace_parking_common_parameters.minimum_turning_radius = - node->declare_parameter(ns + "minimum_turning_radius"); - p.freespace_parking_common_parameters.maximum_turning_radius = - node->declare_parameter(ns + "maximum_turning_radius"); - p.freespace_parking_common_parameters.turning_radius_size = - node->declare_parameter(ns + "turning_radius_size"); - p.freespace_parking_common_parameters.maximum_turning_radius = std::max( - p.freespace_parking_common_parameters.maximum_turning_radius, - p.freespace_parking_common_parameters.minimum_turning_radius); - p.freespace_parking_common_parameters.turning_radius_size = - std::max(p.freespace_parking_common_parameters.turning_radius_size, 1); + p.freespace_parking_common_parameters.max_turning_ratio = + node->declare_parameter(ns + "max_turning_ratio"); + p.freespace_parking_common_parameters.turning_steps = + node->declare_parameter(ns + "turning_steps"); } // freespace parking search config @@ -590,19 +583,10 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "time_limit", p->freespace_parking_common_parameters.time_limit); updateParam( - parameters, ns + "minimum_turning_radius", - p->freespace_parking_common_parameters.minimum_turning_radius); - updateParam( - parameters, ns + "maximum_turning_radius", - p->freespace_parking_common_parameters.maximum_turning_radius); + parameters, ns + "max_turning_ratio", + p->freespace_parking_common_parameters.max_turning_ratio); updateParam( - parameters, ns + "turning_radius_size", - p->freespace_parking_common_parameters.turning_radius_size); - p->freespace_parking_common_parameters.maximum_turning_radius = std::max( - p->freespace_parking_common_parameters.maximum_turning_radius, - p->freespace_parking_common_parameters.minimum_turning_radius); - p->freespace_parking_common_parameters.turning_radius_size = - std::max(p->freespace_parking_common_parameters.turning_radius_size, 1); + parameters, ns + "turning_steps", p->freespace_parking_common_parameters.turning_steps); } // freespace parking search config diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 9570621f0f717..23ca76432859d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -246,10 +246,11 @@ std::optional ShiftPullOver::generatePullOverPath( // set pull over path PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); - pull_over_path.partial_paths.push_back(shifted_path.path); + std::vector partial_paths{shifted_path.path}; + pull_over_path.setPaths( + partial_paths, path_shifter.getShiftLines().front().start, + path_shifter.getShiftLines().front().end); pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); - pull_over_path.start_pose = path_shifter.getShiftLines().front().start; - pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); pull_over_path.debug_poses.push_back(goal_pose); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 5790be377b7aa..63f5a2ec05bf1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -331,8 +331,7 @@ title NormalLaneChange::filterObjects Method Execution Flow start group "Filter Objects by Class" { -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) +while (has not finished iterating through predicted object list) is (TRUE) if (current object type != param.object_types_to_check?) then (TRUE) #LightPink:Remove current object; else (FALSE) @@ -341,17 +340,15 @@ endif end while end group -if (object list is empty?) then (TRUE) +if (predicted object list is empty?) then (TRUE) :Return empty result; stop else (FALSE) endif group "Filter Oncoming Objects" #PowderBlue { -:Iterate through each object in target lane objects list; -while (has not finished iterating through object list?) is (TRUE) -:check object's yaw with reference to ego's yaw.; -if (yaw difference < 90 degree?) then (TRUE) +while (has not finished iterating through predicted object list?) is (TRUE) +if (object's yaw with reference to ego's yaw difference < 90 degree?) then (TRUE) :Keep current object; else (FALSE) if (object is stopping?) then (TRUE) @@ -363,31 +360,7 @@ endif endwhile end group -if (object list is empty?) then (TRUE) - :Return empty result; - stop -else (FALSE) -endif - -group "Filter Objects Ahead Terminal" #Beige { -:Calculate lateral distance from ego to current lanes center; - -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) - :Get current object's polygon; - :Initialize distance to terminal from object to max; - while (has not finished iterating through object polygon's vertices) is (TRUE) - :Calculate object's lateral distance to end of lane; - :Update minimum distance to terminal from object; - end while - if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE) - #LightPink:Remove current object; - else (FALSE) - endif -end while -end group - -if (object list is empty?) then (TRUE) +if (predicted object list is empty?) then (TRUE) :Return empty result; stop else (FALSE) @@ -395,21 +368,27 @@ endif group "Filter Objects By Lanelets" #LightGreen { -:Iterate through each object in objects list; -while (has not finished iterating through object list) is (TRUE) - :lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.; - if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE) - :Add to target_lane_objects; - else (FALSE) - if (Object overlaps with backward target lanes?) then (TRUE) +while (has not finished iterating through predicted object list) is (TRUE) + :Calculate lateral distance diff; + if (Object in target lane polygon?) then (TRUE) + if (lateral distance diff > half of ego's width?) then (TRUE) + if (Object's physical position is before terminal point?) then (TRUE) :Add to target_lane_objects; else (FALSE) - if (Object in current lane polygon?) then (TRUE) - :Add to current_lane_objects; - else (FALSE) - :Add to other_lane_objects; - endif endif + else (FALSE) + endif + else (FALSE) + endif + + if (Object overlaps with backward target lanes?) then (TRUE) + :Add to target_lane_objects; + else (FALSE) + if (Object in current lane polygon?) then (TRUE) + :Add to current_lane_objects; + else (FALSE) + :Add to other_lane_objects; + endif endif end while @@ -426,13 +405,10 @@ endif group "Filter Target Lanes' objects" #LightCyan { -:Iterate through each object in target lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through target lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) :Keep current object; else (FALSE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -444,11 +420,8 @@ end group group "Filter Current Lanes' objects" #LightYellow { -:Iterate through each object in current lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through current lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -462,11 +435,8 @@ end group group "Filter Other Lanes' objects" #Lavender { -:Iterate through each object in other lane objects list; -while (has not finished iterating through object list) is (TRUE) - :check object's velocity; +while (has not finished iterating through other lanes' object list) is (TRUE) if(velocity is within threshold?) then (TRUE) - :check whether object is ahead of ego; if(object is ahead of ego?) then (TRUE) :keep current object; else (FALSE) @@ -478,7 +448,7 @@ while (has not finished iterating through object list) is (TRUE) endwhile end group -:Trasform the objects into extended predicted object and return them as lane_change_target_objects; +:Transform the objects into extended predicted object and return them as lane_change_target_objects; stop @enduml 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 dad96d5f7371a..a1284f355a87d 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 @@ -128,12 +128,8 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - void filterAheadTerminalObjects( - PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const; - void filterObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, std::vector & current_lane_objects, std::vector & target_lane_objects, std::vector & other_lane_objects) const; 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 1bb4dfdeb59dc..495fe9012ecd0 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 @@ -185,6 +185,7 @@ struct PhaseInfo struct Lanes { + bool current_lane_in_goal_section{false}; lanelet::ConstLanelets current; lanelet::ConstLanelets target; std::vector preceding_target; 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 17eaceb055fc1..7bb6a06116a53 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 @@ -136,7 +136,7 @@ PathWithLaneId getReferencePathFromTargetLane( const double next_lane_change_buffer); std::vector generateDrivableLanes( - const std::vector original_drivable_lanes, const RouteHandler & route_handler, + const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); std::vector generateDrivableLanes( @@ -178,10 +178,10 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passParkedObject( +bool passed_parked_objects( 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, CollisionCheckDebugMap & object_debug); + CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, @@ -302,6 +302,18 @@ 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); + +Pose to_pose( + const autoware::universe_utils::Point2d & point, + const geometry_msgs::msg::Quaternion & orientation); + +bool is_ahead_of_ego( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object); + +bool is_before_terminal( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object); } // 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 4823cb0bfec22..b52679d6778ad 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 @@ -124,9 +124,18 @@ BehaviorModuleOutput LaneChangeInterface::plan() } else { const auto path = assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); - updateRTCStatus( - path.start_distance_to_path_change, path.finish_distance_to_path_change, true, - State::RUNNING); + const auto force_activated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); + if (!force_activated) { + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, true, + State::RUNNING); + } else { + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, false, + State::RUNNING); + } } return output; @@ -227,6 +236,15 @@ bool LaneChangeInterface::canTransitFailureState() updateDebugMarker(); log_debug_throttled(__func__); + const auto force_activated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); + + if (force_activated) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed"); + return false; + } + if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { log_debug_throttled("Abort process has on going."); return false; @@ -257,6 +275,19 @@ bool LaneChangeInterface::canTransitFailureState() return true; } + const auto force_deactivated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); }); + + if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("Cancel lane change due to force deactivation"); + module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + return true; + } + if (post_process_safety_status_.is_safe) { log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); return false; 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 4dfba64615504..97eab5a03fcaa 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 @@ -83,8 +83,11 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->current = current_lanes; common_data_ptr_->lanes_ptr->target = target_lanes; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + common_data_ptr_->lanes_ptr->current_lane_in_goal_section = + route_handler_ptr->isInGoalRouteSection(current_lanes.back()); common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( - *common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), + *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_); @@ -983,7 +986,6 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects( LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const { - const auto & current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; auto objects = *planner_data_->dynamic_object; @@ -1006,12 +1008,6 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } - filterAheadTerminalObjects(objects, current_lanes); - - if (objects.objects.empty()) { - return {}; - } - std::vector target_lane_objects; std::vector current_lane_objects; std::vector other_lane_objects; @@ -1022,9 +1018,11 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return {}; } + const auto path = + route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + filterObjectsByLanelets( - objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, - other_lane_objects); + objects, path, current_lane_objects, target_lane_objects, other_lane_objects); const auto is_within_vel_th = [](const auto & object) -> bool { constexpr double min_vel_th = 1.0; @@ -1032,38 +1030,25 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); }; - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - - if (path.points.empty()) { - return {}; - } - - const auto is_ahead_of_ego = [&path, ¤t_pose](const auto & object) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - } - return max_dist_ego_to_obj >= 0.0; - }; - utils::path_safety_checker::filterObjects( target_lane_objects, [&](const PredictedObject & object) { - return (is_within_vel_th(object) || is_ahead_of_ego(object)); + const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) || ahead_of_ego; }); - utils::path_safety_checker::filterObjects( - other_lane_objects, [&](const PredictedObject & object) { - return is_within_vel_th(object) && is_ahead_of_ego(object); - }); + if (lane_change_parameters_->check_objects_on_other_lanes) { + utils::path_safety_checker::filterObjects( + other_lane_objects, [&](const PredictedObject & object) { + const auto ahead_of_ego = + utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) && ahead_of_ego; + }); + } utils::path_safety_checker::filterObjects( current_lane_objects, [&](const PredictedObject & object) { - return is_within_vel_th(object) && is_ahead_of_ego(object); + const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object); + return is_within_vel_th(object) && ahead_of_ego; }); LaneChangeLanesFilteredObjects lane_change_target_objects; @@ -1119,42 +1104,15 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -void NormalLaneChange::filterAheadTerminalObjects( - PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const -{ - const auto & current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, current_lanes.back(), *lane_change_parameters_, direction_); - - const auto dist_ego_to_current_lanes_center = - lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); - - // ignore object that are ahead of terminal lane change start - utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { - const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); - // ignore object that are ahead of terminal lane change start - auto distance_to_terminal_from_object = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon) { - const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - Pose polygon_pose; - polygon_pose.position = obj_p; - polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; - const auto dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); - distance_to_terminal_from_object = std::min(dist_ego_to_current_lanes_center, dist); - } - - return (minimum_lane_change_length > distance_to_terminal_from_object); - }); -} - void NormalLaneChange::filterObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, std::vector & current_lane_objects, + const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path, + std::vector & current_lane_objects, std::vector & target_lane_objects, std::vector & other_lane_objects) const { const auto & current_pose = getEgoPose(); + const auto & current_lanes = common_data_ptr_->lanes_ptr->current; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto & route_handler = getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto check_optional_polygon = [](const auto & object, const auto & polygon) { @@ -1199,7 +1157,14 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) { + const auto is_before_terminal = [&]() { + return utils::lane_change::is_before_terminal( + common_data_ptr_, current_lanes_ref_path, object); + }; + + if ( + check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far && + is_before_terminal()) { target_lane_objects.push_back(object); continue; } @@ -1628,10 +1593,9 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->push_back(*candidate_path); if ( - !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)) { + !is_stuck && !utils::lane_change::passed_parked_objects( + common_data_ptr_, *candidate_path, filtered_objects.target_lane, + lane_change_buffer, 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."); @@ -1815,6 +1779,19 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto target_objects = getTargetObjects(filtered_objects, current_lanes); CollisionCheckDebugMap debug_data; + + const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); + + const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( + common_data_ptr_, path, filtered_objects.target_lane, min_lc_length, debug_data); + + if (!has_passed_parked_objects) { + RCLCPP_DEBUG(logger_, "Lane change has been delayed."); + return {false, false}; + } + const auto safety_status = isLaneChangePathSafe( path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { 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 5f13aa3192113..176a7ece0084e 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 @@ -527,7 +527,7 @@ std::vector generateDrivableLanes( } std::vector generateDrivableLanes( - const std::vector original_drivable_lanes, const RouteHandler & route_handler, + const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) { const auto has_same_lane = @@ -968,10 +968,10 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passParkedObject( +bool passed_parked_objects( 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, CollisionCheckDebugMap & object_debug) + CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -979,20 +979,19 @@ bool passParkedObject( 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 = common_data_ptr->lanes_ptr->current; const auto current_lane_path = route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) { - return false; + if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { + return true; } const auto leading_obj_idx = getLeadingStaticObjectIdx( route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); if (!leading_obj_idx) { - return false; + return true; } const auto & leading_obj = objects.at(*leading_obj_idx); @@ -1000,11 +999,13 @@ bool passParkedObject( const auto leading_obj_poly = autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { - return false; + return true; } const auto & current_path_end = current_lane_path.points.back().point.pose.position; double min_dist_to_end_of_current_lane = std::numeric_limits::max(); + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); for (const auto & point : leading_obj_poly.outer()) { const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); const double dist = autoware::motion_utils::calcSignedArcLength( @@ -1022,10 +1023,10 @@ bool passParkedObject( if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { debug.second.unsafe_reason = "delay lane change"; utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return true; + return false; } - return false; + return true; } std::optional getLeadingStaticObjectIdx( @@ -1215,6 +1216,7 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) lanes_polygon.current = utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); + lanes_polygon.target = utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); @@ -1262,6 +1264,85 @@ bool is_same_lane_with_prev_iteration( return (prev_target_lanes.front().id() == target_lanes.front().id()) && (prev_target_lanes.back().id() == prev_target_lanes.back().id()); } + +Pose to_pose( + const autoware::universe_utils::Point2d & point, + const geometry_msgs::msg::Quaternion & orientation) +{ + Pose pose; + pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); + pose.orientation = orientation; + return pose; +} + +bool is_ahead_of_ego( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object) +{ + const auto & current_ego_pose = common_data_ptr->get_ego_pose(); + + const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + + const auto dist_to_base_link = autoware::motion_utils::calcSignedArcLength( + path.points, current_ego_pose.position, obj_position); + const auto & ego_info = common_data_ptr->bpp_param_ptr->vehicle_info; + const auto lon_dev = std::max( + ego_info.max_longitudinal_offset_m + ego_info.rear_overhang_m, object.shape.dimensions.x); + + // we don't always have to check the distance accurately. + if (std::abs(dist_to_base_link) > lon_dev) { + return dist_to_base_link >= 0.0; + } + + const auto ego_polygon = getEgoCurrentFootprint(current_ego_pose, ego_info).outer(); + auto ego_min_dist_to_end = std::numeric_limits::max(); + for (const auto & ego_edge_point : ego_polygon) { + const auto ego_edge = + autoware::universe_utils::createPoint(ego_edge_point.x(), ego_edge_point.y(), 0.0); + const auto dist_to_end = autoware::motion_utils::calcSignedArcLength( + path.points, ego_edge, path.points.back().point.pose.position); + ego_min_dist_to_end = std::min(dist_to_end, ego_min_dist_to_end); + } + + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); + auto current_min_dist_to_end = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength( + path.points, obj_p, path.points.back().point.pose.position); + current_min_dist_to_end = std::min(dist_ego_to_obj, current_min_dist_to_end); + } + return ego_min_dist_to_end - current_min_dist_to_end >= 0.0; +} + +bool is_before_terminal( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, + const PredictedObject & object) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto & lanes_ptr = common_data_ptr->lanes_ptr; + const auto terminal_position = (lanes_ptr->current_lane_in_goal_section) + ? route_handler_ptr->getGoalPose().position + : path.points.back().point.pose.position; + double current_max_dist = std::numeric_limits::lowest(); + + const auto & obj_position = object.kinematics.initial_pose_with_covariance.pose.position; + const auto dist_to_base_link = + autoware::motion_utils::calcSignedArcLength(path.points, obj_position, terminal_position); + // we don't always have to check the distance accurately. + if (std::abs(dist_to_base_link) > object.shape.dimensions.x) { + return dist_to_base_link >= 0.0; + } + + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto dist_obj_to_terminal = + autoware::motion_utils::calcSignedArcLength(path.points, obj_p, terminal_position); + current_max_dist = std::max(dist_obj_to_terminal, current_max_dist); + } + return current_max_dist >= 0.0; +} } // 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/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml index bb897db393b65..f4f9d131810fd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -10,6 +10,7 @@ enabled: true print_runtime: false max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + min_bound_interval: 0.1 # [m] minimum interval between two consecutive bound points (before expansion) smoothing: curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length 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/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index e92fe937708b8..bf323e76912d4 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 @@ -14,8 +14,6 @@ #include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" -#include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" -#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" @@ -218,7 +216,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"); @@ -230,7 +228,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; @@ -910,6 +908,9 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM, planner_data_->drivable_area_expansion_parameters.arc_length_range); + updateParam( + parameters, DrivableAreaExpansionParameters::MIN_BOUND_INTERVAL, + planner_data_->drivable_area_expansion_parameters.min_bound_interval); updateParam( parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, planner_data_->drivable_area_expansion_parameters.print_runtime); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 95eb82e0b4c23..09e1382799f01 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -335,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 false; + } + + return rtc.second->isActivated(uuid_map_.at(rtc.first)); }); } @@ -345,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)); }); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp index 819800281fcc5..85c2563c5d98c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/marker_utils/utils.hpp @@ -67,7 +67,7 @@ MarkerArray createFootprintMarkerArray( const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( - const std::vector points, const std::string & ns, const int32_t id, const float r, + const std::vector & points, const std::string & ns, const int32_t id, const float r, const float g, const float b); MarkerArray createPoseMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 651b7a0643bca..d3b7da8c4d03f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -58,6 +58,7 @@ struct DrivableAreaExpansionParameters static constexpr auto SMOOTHING_ARC_LENGTH_RANGE_PARAM = "dynamic_expansion.smoothing.arc_length_range"; static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; + static constexpr auto MIN_BOUND_INTERVAL = "dynamic_expansion.min_bound_interval"; // static expansion double drivable_area_right_bound_offset{}; @@ -80,6 +81,7 @@ struct DrivableAreaExpansionParameters double resample_interval{}; double arc_length_range{}; double max_reuse_deviation{}; + double min_bound_interval{}; bool avoid_dynamic_objects{}; bool print_runtime{}; std::vector avoid_linestring_types{}; @@ -119,6 +121,7 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(AVOID_LINESTRING_TYPES_PARAM); avoid_dynamic_objects = node.declare_parameter(AVOID_DYN_OBJECTS_PARAM); avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); + min_bound_interval = node.declare_parameter(MIN_BOUND_INTERVAL); print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index d0d19b6b8fed2..05f7c70fa42c2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -27,6 +27,7 @@ #include #include +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -156,6 +157,12 @@ bool checkSafetyWithIntegralPredictedPolygon( const ExtendedPredictedObjects & objects, const bool check_all_predicted_path, const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map); +double calcObstacleMinLength(const Shape & shape); +double calcObstacleMaxLength(const Shape & shape); +std::pair checkObjectsCollisionRough( + const PathWithLaneId & path, const PredictedObjects & objects, const double margin, + const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point); + // debug CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp index 3b06c7e03cae1..45bdb296f6575 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp @@ -79,7 +79,7 @@ PathWithLaneId convertWayPointsToPathWithLaneId( std::vector getReversingIndices(const PathWithLaneId & path); std::vector dividePath( - const PathWithLaneId & path, const std::vector indices); + const PathWithLaneId & path, const std::vector & indices); void correctDividedPathVelocity(std::vector & divided_paths); @@ -87,8 +87,8 @@ bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double // only two points is supported std::vector splineTwoPoints( - std::vector base_s, std::vector base_x, const double begin_diff, - const double end_diff, std::vector new_s); + const std::vector & base_s, const std::vector & base_x, const double begin_diff, + const double end_diff, const std::vector & new_s); std::vector interpolatePose( const Pose & start_pose, const Pose & end_pose, const double resample_interval); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 8c9b4ef35d5af..d57c2339041f4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -79,7 +79,7 @@ MarkerArray createFootprintMarkerArray( } MarkerArray createPointsMarkerArray( - const std::vector points, const std::string & ns, const int32_t id, const float r, + const std::vector & points, const std::string & ns, const int32_t id, const float r, const float g, const float b) { auto marker = createDefaultMarker( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 6e028cec2ed1b..dcab7c22a35fe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.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. @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include #include @@ -276,7 +278,13 @@ void expand_bound( const auto projection = point_to_linestring_projection(bound_p, path_ls); const auto expansion_ratio = (expansions[idx] + projection.distance) / projection.distance; const auto & path_p = projection.projected_point; - const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); + auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); + // push the bound again if it got too close to another part of the path + const auto new_projection = point_to_linestring_projection(expanded_p, path_ls); + if (new_projection.distance < projection.distance) { + const auto new_expansion_ratio = (projection.distance) / new_projection.distance; + expanded_p = lerp_point(new_projection.projected_point, expanded_p, new_expansion_ratio); + } bound[idx].x = expanded_p.x(); bound[idx].y = expanded_p.y(); } @@ -291,8 +299,8 @@ void expand_bound( bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); if ( intersection && - autoware::universe_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && - autoware::universe_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + autoware::universe_utils::calcDistance2d(*intersection, bound[idx - 1]) > 1e-3 && + autoware::universe_utils::calcDistance2d(*intersection, bound[idx]) > 1e-3) { idx = succ_idx; is_intersecting = true; } @@ -360,6 +368,42 @@ void calculate_expansion_distances( } } +void add_bound_point(std::vector & bound, const Pose & pose, const double min_bound_interval) +{ + const auto p = convert_point(pose.position); + PointDistance nearest_projection; + nearest_projection.distance = std::numeric_limits::infinity(); + size_t nearest_idx = 0UL; + for (auto i = 0UL; i + 1 < bound.size(); ++i) { + const auto prev_p = convert_point(bound[i]); + const auto next_p = convert_point(bound[i + 1]); + const auto projection = point_to_segment_projection(p, prev_p, next_p); + if (projection.distance < nearest_projection.distance) { + nearest_projection = projection; + nearest_idx = i; + } + } + Point new_point; + new_point.x = nearest_projection.point.x(); + new_point.y = nearest_projection.point.y(); + new_point.z = bound[nearest_idx].z; + if ( + universe_utils::calcDistance2d(new_point, bound[nearest_idx]) > min_bound_interval && + universe_utils::calcDistance2d(new_point, bound[nearest_idx + 1]) > min_bound_interval) { + bound.insert(bound.begin() + nearest_idx + 1, new_point); + } +} + +void add_bound_points( + std::vector & left_bound, std::vector & right_bound, + const std::vector & path_poses, const double min_bound_interval) +{ + for (const auto & p : path_poses) { + add_bound_point(left_bound, p, min_bound_interval); + add_bound_point(right_bound, p, min_bound_interval); + } +} + void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data) @@ -375,7 +419,6 @@ void expand_drivable_area( *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); const auto preprocessing_ms = stop_watch.toc("preprocessing"); - stop_watch.tic("crop"); std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; @@ -383,6 +426,7 @@ void expand_drivable_area( reuse_previous_poses( path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); const auto crop_ms = stop_watch.toc("crop"); + add_bound_points(path.left_bound, path.right_bound, path_poses, params.min_bound_interval); stop_watch.tic("curvatures_expansion"); // Only add curvatures for the new points. Curvatures of reused path points are not updated. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index b05d869cca6b5..8d2dd2fb86e29 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -36,6 +36,12 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker namespace bg = boost::geometry; +using autoware::motion_utils::calcLongitudinalOffsetPoint; +using autoware::motion_utils::calcLongitudinalOffsetToSegment; +using autoware::motion_utils::findNearestIndex; +using autoware::motion_utils::findNearestSegmentIndex; +using autoware::universe_utils::calcDistance2d; + void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { Point2d point; @@ -722,4 +728,96 @@ void updateCollisionCheckDebugMap( debug_map.insert(object_debug); } +double calcObstacleMinLength(const Shape & shape) +{ + if (shape.type == Shape::BOUNDING_BOX) { + return std::min(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == Shape::POLYGON) { + double min_length_to_point = std::numeric_limits::max(); + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (min_length_to_point > length_to_point) { + min_length_to_point = length_to_point; + } + } + return min_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +double calcObstacleMaxLength(const Shape & shape) +{ + if (shape.type == Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +std::pair checkObjectsCollisionRough( + const PathWithLaneId & path, const PredictedObjects & objects, const double margin, + const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point) +{ + const auto & points = path.points; + + std::pair has_collision = {false, false}; // {min_distance, max_distance} + for (const auto & object : objects.objects) { + // calculate distance between object center and ego base_link + const Point & object_point = object.kinematics.initial_pose_with_covariance.pose.position; + const double distance = std::invoke([&]() -> double { + if (use_offset_ego_point) { + const size_t nearest_segment_idx = findNearestSegmentIndex(points, object_point); + const double offset_length = + calcLongitudinalOffsetToSegment(points, nearest_segment_idx, object_point); + const auto offset_point = + calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length); + const Point ego_point = + offset_point ? offset_point.value() + : points.at(findNearestIndex(points, object_point)).point.pose.position; + return autoware::universe_utils::calcDistance2d(ego_point, object_point); + } + const Point ego_point = points.at(findNearestIndex(points, object_point)).point.pose.position; + return autoware::universe_utils::calcDistance2d(ego_point, object_point); + }); + + // calculate min and max length from object center to edge + const double object_min_length = calcObstacleMinLength(object.shape); + const double object_max_length = calcObstacleMaxLength(object.shape); + + // calculate min and max length from ego base_link to edge + const auto & p = parameters; + const double ego_min_length = + std::min({p.vehicle_width / 2, p.front_overhang / 2, p.rear_overhang / 2}); + const double ego_max_length = std::max( + std::hypot(p.vehicle_width / 2, p.front_overhang), + std::hypot(p.vehicle_width / 2, p.rear_overhang)); + + // calculate min and max distance between object footprint and ego footprint + const double min_distance = distance - object_max_length - ego_max_length; + const double max_distance = distance - object_min_length - ego_min_length; + + if (min_distance < margin) { + has_collision.first = true; + } + if (max_distance < margin) { + has_collision.second = true; + } + } + return has_collision; +} + } // namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 79b4ba8e16344..7873a4b421297 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -376,7 +376,7 @@ std::vector getReversingIndices(const PathWithLaneId & path) } std::vector dividePath( - const PathWithLaneId & path, const std::vector indices) + const PathWithLaneId & path, const std::vector & indices) { std::vector divided_paths; @@ -445,8 +445,8 @@ bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double // only two points is supported std::vector splineTwoPoints( - std::vector base_s, std::vector base_x, const double begin_diff, - const double end_diff, std::vector new_s) + const std::vector & base_s, const std::vector & base_x, const double begin_diff, + const double end_diff, const std::vector & new_s) { assert(base_s.size() == 2 && base_x.size() == 2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp index b6fee5efb3ef8..21784757e469e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.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. @@ -264,13 +264,11 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) autoware::behavior_path_planner::drivable_area_expansion::expand_drivable_area( path, std::make_shared(planner_data)); // expanded left bound - ASSERT_EQ(path.left_bound.size(), 3ul); - EXPECT_GT(path.left_bound[0].y, 1.0); - EXPECT_GT(path.left_bound[1].y, 1.0); - EXPECT_GT(path.left_bound[2].y, 1.0); + for (const auto & p : path.left_bound) { + EXPECT_GT(p.y, 1.0); + } // expanded right bound - ASSERT_EQ(path.right_bound.size(), 3ul); - EXPECT_LT(path.right_bound[0].y, -1.0); - EXPECT_LT(path.right_bound[1].y, -1.0); - EXPECT_LT(path.right_bound[2].y, -1.0); + for (const auto & p : path.right_bound) { + EXPECT_LT(p.y, -1.0); + } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md index dba4a0cfce5e7..5b68b021f99cb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md @@ -469,17 +469,18 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral #### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | -| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | -| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | -| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | +| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | +| maximum_curvature | [1/m] | double | maximum curvature. Calculate the required pull out distance from this maximum curvature, assuming the reference path is considered a straight line and shifted by two approximate arcs. This does not compensate for curvature in a shifted path or curve. | 0.07 | +| end_pose_curvature_threshold | [1/m] | double | The curvature threshold which is used for calculating the shift pull out distance. The shift end pose is shifted forward so that curvature on shift end pose is less than this value. This is to prevent the generated path from having a large curvature when the end pose is on a curve. If a shift end pose with a curvature below the threshold is not found, the shift pull out distance is used as the distance to the point with the lowest curvature among the points beyond a certain distance. | 0.01 | ### **geometric pull out** diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index d39a320a19e14..851e96f7a265c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -32,6 +32,7 @@ minimum_lateral_acc: 0.15 maximum_lateral_acc: 0.5 maximum_curvature: 0.07 + end_pose_curvature_threshold: 0.01 # geometric pull out enable_geometric_pull_out: true geometric_collision_check_distance_from_end: 0.0 @@ -61,9 +62,8 @@ velocity: 1.0 vehicle_shape_margin: 1.0 time_limit: 3000.0 - minimum_turning_radius: 5.0 - maximum_turning_radius: 5.0 - turning_radius_size: 1 + max_turning_ratio: 0.7 + turning_steps: 1 # search configs search_configs: theta_size: 144 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 6eea3e3576732..5903a691d60e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -90,6 +90,9 @@ struct StartPlannerDebugData std::vector start_pose_candidates; size_t selected_start_pose_candidate_index; double margin_for_start_pose_candidate; + + // for isPreventingRearVehicleFromPassingThrough + std::optional estimated_stop_pose; }; struct StartPlannerParameters @@ -119,6 +122,7 @@ struct StartPlannerParameters double maximum_lateral_acc{0.0}; double minimum_lateral_acc{0.0}; double maximum_curvature{0.0}; // maximum curvature considered in the path generation + double end_pose_curvature_threshold{0.0}; double maximum_longitudinal_deviation{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index 145ca5175762e..e358e35ed7794 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -36,7 +37,8 @@ class FreespacePullOut : public PullOutPlannerBase public: FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + std::shared_ptr time_keeper); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index c63dd392a5463..70ca2bdb10d37 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include @@ -33,7 +34,8 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, const std::shared_ptr - lane_departure_checker); + lane_departure_checker, + std::shared_ptr time_keeper); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index 03b6e13cb2d3e..de2e57ff23576 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -20,6 +20,7 @@ #include "autoware/behavior_path_start_planner_module/data_structs.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/util.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -37,7 +38,10 @@ using tier4_planning_msgs::msg::PathWithLaneId; class PullOutPlannerBase { public: - explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) + explicit PullOutPlannerBase( + rclcpp::Node & node, const StartPlannerParameters & parameters, + std::shared_ptr time_keeper) + : time_keeper_(time_keeper) { vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); @@ -63,6 +67,8 @@ class PullOutPlannerBase autoware::behavior_path_planner::PullOutPath & pull_out_path, double collision_check_distance_from_end) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // check for collisions const auto & dynamic_objects = planner_data_->dynamic_object; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -95,6 +101,8 @@ class PullOutPlannerBase LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; double collision_check_margin_; + + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 92cad1c2eb8b8..d1b4c9dabdd7f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -17,6 +17,7 @@ #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include @@ -34,7 +35,8 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker); + std::shared_ptr & lane_departure_checker, + std::shared_ptr time_keeper); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index b07d6497463ef..bb70ae2638056 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -229,8 +229,31 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; + + /** + * @brief Check if the ego vehicle is preventing the rear vehicle from passing through. + * + * This function just call isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) with + * two poses. If rear vehicle is obstructed by ego vehicle at either of the two poses, it returns + * true. + * + * @return true if the ego vehicle is preventing the rear vehicle from passing through with the + * current pose or the pose if it stops. + */ bool isPreventingRearVehicleFromPassingThrough() const; + /** + * @brief Check if the ego vehicle is preventing the rear vehicle from passing through. + * + * This function measures the distance to the lane boundary from the current pose and the pose if +it stops, and determines whether there is enough space for the rear vehicle to pass through. If + * it is obstructing at either of the two poses, it returns true. + * + * @return true if the ego vehicle is preventing the rear vehicle from passing through with given +ego pose. + */ + bool isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const; + bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 9c9c084f823c4..c3f5e8883c284 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -28,8 +28,10 @@ namespace autoware::behavior_path_planner { FreespacePullOut::FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) -: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + std::shared_ptr time_keeper) +: PullOutPlannerBase{node, parameters, time_keeper}, + velocity_{parameters.freespace_planner_velocity} { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); @@ -54,9 +56,12 @@ std::optional FreespacePullOut::plan( planner_->setMap(*planner_data_->costmap); - const bool found_path = planner_->makePlan(start_pose, end_pose); - if (!found_path) { - planner_debug_data.conditions_evaluation.emplace_back("no path found"); + try { + if (!planner_->makePlan(start_pose, end_pose)) { + planner_debug_data.conditions_evaluation.emplace_back("no path found"); + return {}; + } + } catch (const std::exception & e) { return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 4470026bc8dda..13a9908b97daf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -33,8 +33,9 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, const std::shared_ptr - lane_departure_checker) -: PullOutPlannerBase{node, parameters}, + lane_departure_checker, + std::shared_ptr time_keeper) +: PullOutPlannerBase{node, parameters, time_keeper}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_(lane_departure_checker) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 873bab1043485..23817ee081501 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -31,21 +31,78 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) StartPlannerParameters p; - const std::string ns = "start_planner."; - - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); - p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); - p.th_distance_to_middle_of_the_road = - node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); - p.extra_width_margin_for_rear_obstacle = - node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); - p.collision_check_margins = - node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_margin_from_front_object = - node->declare_parameter(ns + "collision_check_margin_from_front_object"); - p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + { + const std::string ns = "start_planner."; + + p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); + p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); + p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); + p.th_distance_to_middle_of_the_road = + node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_margins = + node->declare_parameter>(ns + "collision_check_margins"); + p.collision_check_margin_from_front_object = + node->declare_parameter(ns + "collision_check_margin_from_front_object"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); + // shift pull out + p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); + p.shift_collision_check_distance_from_end = + node->declare_parameter(ns + "shift_collision_check_distance_from_end"); + p.minimum_shift_pull_out_distance = + node->declare_parameter(ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + node->declare_parameter(ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); + p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); + p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); + p.end_pose_curvature_threshold = + node->declare_parameter(ns + "end_pose_curvature_threshold"); + p.maximum_longitudinal_deviation = + node->declare_parameter(ns + "maximum_longitudinal_deviation"); + // geometric pull out + p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + node->declare_parameter(ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_arc_path_interval = + node->declare_parameter(ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + node->declare_parameter(ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking + // search start pose backward + p.search_priority = node->declare_parameter( + ns + "search_priority"); // "efficient_path" or "short_back_distance" + p.enable_back = node->declare_parameter(ns + "enable_back"); + p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); + p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); + p.backward_search_resolution = + node->declare_parameter(ns + "backward_search_resolution"); + p.backward_path_update_duration = + node->declare_parameter(ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // stop condition + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } { const std::string ns = "start_planner.object_types_to_check_for_path_generation."; p.object_types_to_check_for_path_generation.check_car = @@ -65,53 +122,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.object_types_to_check_for_path_generation.check_pedestrian = node->declare_parameter(ns + "check_pedestrian"); } - p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); - // shift pull out - p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); - p.check_shift_path_lane_departure = - node->declare_parameter(ns + "check_shift_path_lane_departure"); - p.allow_check_shift_path_lane_departure_override = - node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); - p.shift_collision_check_distance_from_end = - node->declare_parameter(ns + "shift_collision_check_distance_from_end"); - p.minimum_shift_pull_out_distance = - node->declare_parameter(ns + "minimum_shift_pull_out_distance"); - p.lateral_acceleration_sampling_num = - node->declare_parameter(ns + "lateral_acceleration_sampling_num"); - p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); - p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); - p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); - p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); - p.maximum_longitudinal_deviation = - node->declare_parameter(ns + "maximum_longitudinal_deviation"); - // geometric pull out - p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); - p.geometric_collision_check_distance_from_end = - node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); - p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); - p.parallel_parking_parameters.pull_out_velocity = - node->declare_parameter(ns + "geometric_pull_out_velocity"); - p.parallel_parking_parameters.pull_out_arc_path_interval = - node->declare_parameter(ns + "arc_path_interval"); - p.parallel_parking_parameters.pull_out_lane_departure_margin = - node->declare_parameter(ns + "lane_departure_margin"); - p.lane_departure_check_expansion_margin = - node->declare_parameter(ns + "lane_departure_check_expansion_margin"); - p.parallel_parking_parameters.pull_out_max_steer_angle = - node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg - p.parallel_parking_parameters.center_line_path_interval = - p.center_line_path_interval; // for geometric parallel parking - // search start pose backward - p.search_priority = node->declare_parameter( - ns + "search_priority"); // "efficient_path" or "short_back_distance" - p.enable_back = node->declare_parameter(ns + "enable_back"); - p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); - p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); - p.backward_search_resolution = node->declare_parameter(ns + "backward_search_resolution"); - p.backward_path_update_duration = - node->declare_parameter(ns + "backward_path_update_duration"); - p.ignore_distance_from_lane_end = - node->declare_parameter(ns + "ignore_distance_from_lane_end"); // freespace planner general params { const std::string ns = "start_planner.freespace_planner."; @@ -127,17 +137,10 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); p.freespace_planner_common_parameters.time_limit = node->declare_parameter(ns + "time_limit"); - p.freespace_planner_common_parameters.minimum_turning_radius = - node->declare_parameter(ns + "minimum_turning_radius"); - p.freespace_planner_common_parameters.maximum_turning_radius = - node->declare_parameter(ns + "maximum_turning_radius"); - p.freespace_planner_common_parameters.turning_radius_size = - node->declare_parameter(ns + "turning_radius_size"); - p.freespace_planner_common_parameters.maximum_turning_radius = std::max( - p.freespace_planner_common_parameters.maximum_turning_radius, - p.freespace_planner_common_parameters.minimum_turning_radius); - p.freespace_planner_common_parameters.turning_radius_size = - std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + p.freespace_planner_common_parameters.max_turning_ratio = + node->declare_parameter(ns + "max_turning_ratio"); + p.freespace_planner_common_parameters.turning_steps = + node->declare_parameter(ns + "turning_steps"); } // freespace planner search config { @@ -182,19 +185,10 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } - // stop condition - { - p.maximum_deceleration_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); - p.maximum_jerk_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); - } - const std::string base_ns = "start_planner.path_safety_check."; - // EgoPredictedPath - const std::string ego_path_ns = base_ns + "ego_predicted_path."; { + const std::string ego_path_ns = base_ns + "ego_predicted_path."; p.ego_predicted_path_params.min_velocity = node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = @@ -208,7 +202,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.ego_predicted_path_params.delay_until_departure = node->declare_parameter(ego_path_ns + "delay_until_departure"); } - // ObjectFilteringParams const std::string obj_filter_ns = base_ns + "target_filtering."; { @@ -233,10 +226,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.objects_filtering_params.use_predicted_path_outside_lanelet = node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); } - // ObjectTypesToCheck - const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; { + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; p.objects_filtering_params.object_types_to_check.check_car = node->declare_parameter(obj_types_ns + "check_car"); p.objects_filtering_params.object_types_to_check.check_truck = @@ -254,10 +246,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.objects_filtering_params.object_types_to_check.check_pedestrian = node->declare_parameter(obj_types_ns + "check_pedestrian"); } - // ObjectLaneConfiguration - const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; p.objects_filtering_params.object_lane_configuration.check_current_lane = node->declare_parameter(obj_lane_ns + "check_current_lane"); p.objects_filtering_params.object_lane_configuration.check_right_lane = @@ -269,7 +260,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.objects_filtering_params.object_lane_configuration.check_other_lane = node->declare_parameter(obj_lane_ns + "check_other_lane"); } - // SafetyCheckParams const std::string safety_check_ns = base_ns + "safety_check_params."; { @@ -286,10 +276,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.safety_check_params.collision_check_yaw_diff_threshold = node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); } - // RSSparams - const std::string rss_ns = safety_check_ns + "rss_params."; { + const std::string rss_ns = safety_check_ns + "rss_params."; p.safety_check_params.rss_params.rear_vehicle_reaction_time = node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = @@ -303,17 +292,17 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.safety_check_params.rss_params.extended_polygon_policy = node->declare_parameter(rss_ns + "extended_polygon_policy"); } - // surround moving obstacle check - std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; p.search_radius = node->declare_parameter(surround_moving_obstacle_check_ns + "search_radius"); p.th_moving_obstacle_velocity = node->declare_parameter( surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); // ObjectTypesToCheck - std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; { + const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; p.surround_moving_obstacles_type_to_check.check_car = node->declare_parameter(obj_types_ns + "check_car"); p.surround_moving_obstacles_type_to_check.check_truck = @@ -334,8 +323,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) } // debug - std::string debug_ns = ns + "debug."; { + const std::string debug_ns = "start_planner.debug."; p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); } @@ -359,9 +348,8 @@ void StartPlannerModuleManager::updateModuleParams( auto & p = parameters_; - const std::string ns = "start_planner."; - { + const std::string ns = "start_planner."; updateParam(parameters, ns + "th_arrived_distance", p->th_arrived_distance); updateParam(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); updateParam(parameters, ns + "th_stopped_time", p->th_stopped_time); @@ -417,6 +405,8 @@ void StartPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); + updateParam( + parameters, ns + "end_pose_curvature_threshold", p->end_pose_curvature_threshold); updateParam( parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); @@ -454,6 +444,11 @@ void StartPlannerModuleManager::updateModuleParams( parameters, ns + "backward_path_update_duration", p->backward_path_update_duration); updateParam( parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end); + updateParam( + parameters, ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); } { const std::string ns = "start_planner.freespace_planner."; @@ -471,19 +466,10 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit); updateParam( - parameters, ns + "minimum_turning_radius", - p->freespace_planner_common_parameters.minimum_turning_radius); - updateParam( - parameters, ns + "maximum_turning_radius", - p->freespace_planner_common_parameters.maximum_turning_radius); + parameters, ns + "max_turning_ratio", + p->freespace_planner_common_parameters.max_turning_ratio); updateParam( - parameters, ns + "turning_radius_size", - p->freespace_planner_common_parameters.turning_radius_size); - p->freespace_planner_common_parameters.maximum_turning_radius = std::max( - p->freespace_planner_common_parameters.maximum_turning_radius, - p->freespace_planner_common_parameters.minimum_turning_radius); - p->freespace_planner_common_parameters.turning_radius_size = - std::max(p->freespace_planner_common_parameters.turning_radius_size, 1); + parameters, ns + "turning_steps", p->freespace_planner_common_parameters.turning_steps); } { const std::string ns = "start_planner.freespace_planner.search_configs."; @@ -521,6 +507,7 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); } + { const std::string ns = "start_planner.freespace_planner.rrtstar."; @@ -533,14 +520,6 @@ void StartPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); } - { - updateParam( - parameters, ns + "stop_condition.maximum_deceleration_for_stop", - p->maximum_deceleration_for_stop); - updateParam( - parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); - } - const std::string base_ns = "start_planner.path_safety_check."; const std::string ego_path_ns = base_ns + "ego_predicted_path."; @@ -563,7 +542,6 @@ void StartPlannerModuleManager::updateModuleParams( } const std::string obj_filter_ns = base_ns + "target_filtering."; - { updateParam( parameters, obj_filter_ns + "safety_check_time_horizon", @@ -597,9 +575,8 @@ void StartPlannerModuleManager::updateModuleParams( p->objects_filtering_params.use_predicted_path_outside_lanelet); } - const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; - { + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; updateParam( parameters, obj_types_ns + "check_car", p->objects_filtering_params.object_types_to_check.check_car); @@ -625,8 +602,8 @@ void StartPlannerModuleManager::updateModuleParams( parameters, obj_types_ns + "check_pedestrian", p->objects_filtering_params.object_types_to_check.check_pedestrian); } - const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { updateParam( parameters, obj_lane_ns + "check_current_lane", @@ -644,6 +621,7 @@ void StartPlannerModuleManager::updateModuleParams( parameters, obj_lane_ns + "check_other_lane", p->objects_filtering_params.object_lane_configuration.check_other_lane); } + const std::string safety_check_ns = base_ns + "safety_check_params."; { updateParam( @@ -665,8 +643,9 @@ void StartPlannerModuleManager::updateModuleParams( parameters, safety_check_ns + "collision_check_yaw_diff_threshold", p->safety_check_params.collision_check_yaw_diff_threshold); } - const std::string rss_ns = safety_check_ns + "rss_params."; + { + const std::string rss_ns = safety_check_ns + "rss_params."; updateParam( parameters, rss_ns + "rear_vehicle_reaction_time", p->safety_check_params.rss_params.rear_vehicle_reaction_time); @@ -686,8 +665,9 @@ void StartPlannerModuleManager::updateModuleParams( parameters, rss_ns + "extended_polygon_policy", p->safety_check_params.rss_params.extended_polygon_policy); } - std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; updateParam( parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius); updateParam( @@ -695,8 +675,8 @@ void StartPlannerModuleManager::updateModuleParams( p->th_moving_obstacle_velocity); // ObjectTypesToCheck - std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; { + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; updateParam( parameters, obj_types_ns + "check_car", p->surround_moving_obstacles_type_to_check.check_car); @@ -724,8 +704,8 @@ void StartPlannerModuleManager::updateModuleParams( } } - std::string debug_ns = ns + "debug."; { + const std::string debug_ns = "start_planner.debug."; updateParam(parameters, debug_ns + "print_debug_info", p->print_debug_info); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index f1e0f8abb2395..2b63ff6df0db2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -37,8 +37,9 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker) -: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker} + std::shared_ptr & lane_departure_checker, + std::shared_ptr time_keeper) +: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker} { } @@ -62,6 +63,8 @@ std::optional ShiftPullOut::plan( // get safe path for (auto & pull_out_path : pull_out_paths) { + universe_utils::ScopedTimeTrack st("get safe path", *time_keeper_); + // shift path is not separate but only one. auto & shift_path = pull_out_path.partial_paths.front(); // check lane_departure with path between pull_out_start to pull_out_end @@ -215,6 +218,8 @@ std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, const Pose & goal_pose) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::vector candidate_paths{}; if (road_lanes.empty()) { @@ -229,6 +234,7 @@ std::vector ShiftPullOut::calcPullOutPaths( const double minimum_lateral_acc = parameters_.minimum_lateral_acc; const double maximum_lateral_acc = parameters_.maximum_lateral_acc; const double maximum_curvature = parameters_.maximum_curvature; + const double end_pose_curvature_threshold = parameters_.end_pose_curvature_threshold; const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance; const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num; @@ -304,10 +310,58 @@ std::vector ShiftPullOut::calcPullOutPaths( // short length, take maximum with the original distance. // TODO(kosuke55): update the conversion function and get rid of the comparison with original // distance. - const double pull_out_distance_converted = calcBeforeShiftedArcLength( - road_lane_reference_path_from_ego, pull_out_distance, shift_length); - const double before_shifted_pull_out_distance = - std::max(pull_out_distance, pull_out_distance_converted); + const double pull_out_distance_converted = std::max( + pull_out_distance, calcBeforeShiftedArcLength( + road_lane_reference_path_from_ego, pull_out_distance, shift_length)); + + // Calculate the distance until the curvature at end_pose is below a certain threshold. + // This is to prevent the path curvature from becoming unnecessarily large when end_pose is on a + // curve. + const double before_shifted_pull_out_distance = std::invoke([&]() -> double { + double arc_length = 0.0; + + // If a curvature below end_pose_curvature_threshold is not found, return the distance to the + // point with the smallest curvature after pull_out_distance_converted. pull_out_distance is a + // variable to store that distance. + double pull_out_distance = pull_out_distance_converted; + double min_curvature_after_distance_converted = std::numeric_limits::max(); + + const auto curvatures_and_segment_lengths = + autoware::motion_utils::calcCurvatureAndSegmentLength( + road_lane_reference_path_from_ego.points); + + const auto update_arc_length = [&](size_t i, const auto & segment_length) { + arc_length += (i == curvatures_and_segment_lengths.size() - 1) + ? segment_length.first + segment_length.second + : segment_length.first; + }; + + const auto update_min_curvature_and_pull_out_distance = [&](double curvature) { + min_curvature_after_distance_converted = curvature; + pull_out_distance = arc_length; + }; + + for (size_t i = 0; i < curvatures_and_segment_lengths.size(); ++i) { + const auto & [signed_curvature, segment_length] = curvatures_and_segment_lengths[i]; + const double curvature = std::abs(signed_curvature); + update_arc_length(i, segment_length); + if (arc_length > pull_out_distance_converted) { + // update distance with minimum curvature after pull_out_distance_converted + if (curvature < min_curvature_after_distance_converted) { + update_min_curvature_and_pull_out_distance(curvature); + } + // if curvature is smaller than end_pose_curvature_threshold, return the length + if (curvature < end_pose_curvature_threshold) { + return arc_length; + } + } + } + + // if not found point with curvature below end_pose_curvature_threshold + // pull_out_distance_converted, return the distance to the point with the smallest curvature + // after pull_out_distance_converted + return pull_out_distance; + }); // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted if ( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9b94d3505c405..5c3c62b947af2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -65,7 +65,7 @@ StartPlannerModule::StartPlannerModule( vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { - lane_departure_checker_ = std::make_shared(); + lane_departure_checker_ = std::make_shared(time_keeper_); lane_departure_checker_->setVehicleInfo(vehicle_info_); autoware::lane_departure_checker::Param lane_departure_checker_params{}; lane_departure_checker_params.footprint_extra_margin = @@ -76,18 +76,19 @@ StartPlannerModule::StartPlannerModule( // set enabled planner if (parameters_->enable_shift_pull_out) { start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_)); + std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); } if (parameters_->enable_geometric_pull_out) { start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_)); + std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } if (parameters_->enable_freespace_planner) { - freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + freespace_planner_ = + std::make_unique(node, *parameters, vehicle_info_, time_keeper_); const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); freespace_planner_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -211,6 +212,8 @@ void StartPlannerModule::updateObjectsFilteringParams( void StartPlannerModule::updateData() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // The method PlannerManager::run() calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput() before this module's run() method is called // with module_ptr->run(). Then module_ptr->run() invokes StartPlannerModule::updateData and, @@ -368,7 +371,39 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // prepare poses for preventing check + // - current_pose + // - estimated_stop_pose (The position assumed when stopped with the minimum stop distance, + // although it is NOT actually stopped) const auto & current_pose = planner_data_->self_odometry->pose.pose; + std::vector preventing_check_pose{current_pose}; + const auto min_stop_distance = + autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop, + 0.0); + debug_data_.estimated_stop_pose.reset(); // debug + if (min_stop_distance.has_value()) { + const auto current_path = getCurrentPath(); + const auto estimated_stop_pose = calcLongitudinalOffsetPose( + current_path.points, current_pose.position, min_stop_distance.value()); + if (estimated_stop_pose.has_value()) { + preventing_check_pose.push_back(estimated_stop_pose.value()); + debug_data_.estimated_stop_pose = estimated_stop_pose.value(); // debug + } + } + + // check if any of the preventing check poses are preventing rear vehicle from passing through + return std::any_of( + preventing_check_pose.begin(), preventing_check_pose.end(), + [&](const auto & pose) { return isPreventingRearVehicleFromPassingThrough(pose); }); +} + +bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -414,8 +449,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const geometry_msgs::msg::Pose & ego_overhang_point_as_pose, const bool ego_is_merging_from_the_left) -> std::optional> { const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, autoware::universe_utils::pose2transform(current_pose)); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits::max(); @@ -481,7 +516,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // Check backwards just in case the Vehicle behind ego is in a different lanelet constexpr double backwards_length = 200.0; const auto prev_lanes = autoware::behavior_path_planner::utils::getBackwardLanelets( - *route_handler, target_lanes, current_pose, backwards_length); + *route_handler, target_lanes, ego_pose, backwards_length); // return all the relevant lanelets lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); @@ -491,7 +526,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // filtering objects with velocity, position and class const auto filtered_objects = utils::path_safety_checker::filterObjects( - dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position, + dynamic_object, route_handler, relevant_lanelets.value(), ego_pose.position, objects_filtering_params_); if (filtered_objects.objects.empty()) return false; @@ -508,7 +543,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { const auto arc_length = autoware::motion_utils::calcSignedArcLength( - centerline_path.points, current_pose.position, o.initial_pose.pose.position); + centerline_path.points, ego_pose.position, o.initial_pose.pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; arc_length_to_closet_object = arc_length; @@ -618,13 +653,14 @@ bool StartPlannerModule::canTransitSuccessState() BehaviorModuleOutput StartPlannerModule::plan() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); resetPathReference(); } - BehaviorModuleOutput output; if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); @@ -635,6 +671,8 @@ BehaviorModuleOutput StartPlannerModule::plan() } const auto path = std::invoke([&]() { + universe_utils::ScopedTimeTrack st2("plan path", *time_keeper_); + if (!status_.driving_forward && !status_.backward_driving_complete) { return status_.backward_path; } @@ -674,6 +712,7 @@ BehaviorModuleOutput StartPlannerModule::plan() return getCurrentPath(); }); + BehaviorModuleOutput output; output.path = path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); @@ -747,9 +786,10 @@ PathWithLaneId StartPlannerModule::getFullPath() const BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + updatePullOutStatus(); - BehaviorModuleOutput output; if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); @@ -778,6 +818,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } + BehaviorModuleOutput output; output.path = stop_path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); @@ -840,6 +881,8 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, const Pose & goal_pose, const std::string & search_priority) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (start_pose_candidates.empty()) return; auto get_accumulated_debug_stream = [](const std::vector & debug_data_vector) { @@ -856,18 +899,22 @@ void StartPlannerModule::planWithPriority( determinePriorityOrder(search_priority, start_pose_candidates.size()); std::vector debug_data_vector; - for (const auto & collision_check_margin : parameters_->collision_check_margins) { - for (const auto & [index, planner] : order_priority) { - if (findPullOutPath( - start_pose_candidates[index], planner, refined_start_pose, goal_pose, - collision_check_margin, debug_data_vector)) { - debug_data_.selected_start_pose_candidate_index = index; - debug_data_.margin_for_start_pose_candidate = collision_check_margin; - if (parameters_->print_debug_info) { - const auto ss = get_accumulated_debug_stream(debug_data_vector); - DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); + { // create a scope for the scoped time track + universe_utils::ScopedTimeTrack st2("findPullOutPaths", *time_keeper_); + + for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath( + start_pose_candidates[index], planner, refined_start_pose, goal_pose, + collision_check_margin, debug_data_vector)) { + debug_data_.selected_start_pose_candidate_index = index; + debug_data_.margin_for_start_pose_candidate = collision_check_margin; + if (parameters_->print_debug_info) { + const auto ss = get_accumulated_debug_stream(debug_data_vector); + DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); + } + return; } - return; } } } @@ -882,6 +929,8 @@ void StartPlannerModule::planWithPriority( PriorityOrder StartPlannerModule::determinePriorityOrder( const std::string & search_priority, const size_t start_pose_candidates_num) { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + PriorityOrder order_priority; if (search_priority == "efficient_path") { for (const auto & planner : start_planners_) { @@ -1052,6 +1101,8 @@ std::vector StartPlannerModule::generateDrivableLanes( void StartPlannerModule::updatePullOutStatus() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // skip updating if enough time has not passed for preventing chattering between back and // start_planner if (!receivedNewRoute()) { @@ -1120,6 +1171,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -1147,6 +1200,8 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const std::vector StartPlannerModule::searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + std::vector pull_out_start_pose_candidates{}; const auto start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto local_vehicle_footprint = vehicle_info_.createFootprint(); @@ -1211,6 +1266,8 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( const double velocity_threshold, const double object_check_forward_distance, const double object_check_backward_distance) const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); @@ -1360,6 +1417,8 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() bool StartPlannerModule::isSafePath() const { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // TODO(Sugahara): should safety check for backward path const auto pull_out_path = getCurrentPath(); @@ -1719,6 +1778,16 @@ void StartPlannerModule::setDebugData() info_marker_); add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_); + // visualize estimated_stop_pose for isPreventingRearVehicleFromPassingThrough() + if (debug_data_.estimated_stop_pose.has_value()) { + auto footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "estimated_stop_pose", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), purple_color); + footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + addFootprintMarker(footprint_marker, debug_data_.estimated_stop_pose.value(), vehicle_info_); + debug_marker_.markers.push_back(footprint_marker); + } + // set objects of interest for (const auto & [uuid, data] : debug_data_.collision_check) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; 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 0e60e3216361d..f10f871ca3cc0 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 @@ -243,6 +243,8 @@ # For cancel maneuver cancel: enable: true # [-] + force: + duration_time: 2.0 # [s] # For yield maneuver yield: 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 4e14dc4886768..35b547da1f9f6 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 @@ -107,6 +107,8 @@ struct AvoidanceParameters // if this param is true, it reverts avoidance path when the path is no longer needed. bool enable_cancel_maneuver{false}; + double force_deactivate_duration_time{0.0}; + // enable avoidance for all parking vehicle std::string policy_ambiguous_vehicle{"ignore"}; @@ -581,6 +583,8 @@ struct AvoidancePlanningData bool found_avoidance_path{false}; + bool force_deactivated{false}; + double to_stop_line{std::numeric_limits::max()}; double to_start_point{std::numeric_limits::lowest()}; 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 56ac3d7f4f1bb..cc7254380cf0b 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 @@ -301,6 +301,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.cancel."; p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable"); + p.force_deactivate_duration_time = + getOrDeclareParameter(*node, ns + "force.duration_time"); } // yield 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..ea12c66859a36 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}, @@ -462,6 +477,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; + + bool force_deactivated_{false}; + rclcpp::Time last_deactivation_triggered_time_; }; } // namespace autoware::behavior_path_planner 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 38a91ac39525b..2a581f90ab255 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 @@ -1263,6 +1263,16 @@ "type": "boolean", "description": "Flag to enable cancel maneuver.", "default": "true" + }, + "force": { + "type": "object", + "properties": { + "duration_time": { + "type": "number", + "description": "force deactivate duration time", + "default": 2.0 + } + } } }, "required": ["enable"], 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 5655696ff086d..015a82aea3013 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 @@ -182,6 +182,11 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang); } + { + const std::string ns = "avoidance.cancel."; + updateParam(parameters, ns + "force.duration_time", p->force_deactivate_duration_time); + } + { const std::string ns = "avoidance.stop."; updateParam(parameters, ns + "max_distance", p->stop_max_distance); 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 b5b9378618a60..e28bd136a0681 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 @@ -570,6 +570,24 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( return; } + const auto registered_sl_force_deactivated = + [&](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)->isForceDeactivated(shift_line.uuid); + }); + }; + + const auto is_force_deactivated = registered_sl_force_deactivated("left", left_shift_array_) || + registered_sl_force_deactivated("right", right_shift_array_); + if (is_force_deactivated && can_yield_maneuver) { + data.yield_required = true; + data.safe_shift_line = data.new_shift_line; + data.force_deactivated = true; + RCLCPP_INFO(getLogger(), "this module is force deactivated. wait until reactivation"); + return; + } + /** * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation. */ @@ -579,6 +597,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. @@ -716,6 +773,10 @@ bool StaticObstacleAvoidanceModule::isSafePath( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data_->parameters; + if (force_deactivated_) { + return false; + } + if (!parameters_->enable_safety_check) { return true; // if safety check is disabled, it always return safe. } @@ -1332,6 +1393,19 @@ void StaticObstacleAvoidanceModule::updateData() } safe_ = avoid_data_.safe; + + if (!force_deactivated_) { + last_deactivation_triggered_time_ = clock_->now(); + force_deactivated_ = avoid_data_.force_deactivated; + return; + } + + if ( + (clock_->now() - last_deactivation_triggered_time_).seconds() > + parameters_->force_deactivate_duration_time) { + RCLCPP_INFO(getLogger(), "The force deactivation is released"); + force_deactivated_ = false; + } } void StaticObstacleAvoidanceModule::processOnEntry() 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..57a6e340bf853 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); @@ -807,6 +827,8 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( data, debug.step1_front_shift_line); + applySmallShiftFilter(ret, 1e-3); + return ret; } 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 ca1ee624a54d5..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 @@ -886,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; @@ -1101,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; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp index 7faf78cb2d979..0c913964332ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.hpp @@ -168,7 +168,7 @@ class BlindSpotModule : public SceneModuleInterface tier4_planning_msgs::msg::PathWithLaneId * path) const; std::optional isOverPassJudge( - const tier4_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const geometry_msgs::msg::Pose & stop_point_pose) const; double computeTimeToPassStopLine( @@ -215,7 +215,7 @@ class BlindSpotModule : public SceneModuleInterface std::optional generateBlindSpotPolygons( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, - const geometry_msgs::msg::Pose & pose) const; + const geometry_msgs::msg::Pose & stop_line_pose) const; /** * @brief Check if object is belong to targeted classes @@ -231,7 +231,8 @@ class BlindSpotModule : public SceneModuleInterface */ autoware_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( const std_msgs::msg::Header & header, - const autoware_perception_msgs::msg::PredictedObject & object, const double time_thr) const; + const autoware_perception_msgs::msg::PredictedObject & object_original, + const double time_thr) const; StateMachine state_machine_; //! for state 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/occluded_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 55aa4daed9f87..6b78a72e887d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -86,7 +86,7 @@ std::vector calculate_detection_areas( std::vector select_and_inflate_objects( const std::vector & objects, - const std::vector velocity_thresholds, const double inflate_size) + const std::vector & velocity_thresholds, const double inflate_size) { std::vector selected_objects; for (const auto & o : objects) { 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 1e6658987f522..ca476481ddde5 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; }; @@ -143,7 +145,7 @@ std::vector calcOverlappingPoints(const Polygon2d & polygon1, const P StopFactor createStopFactor( const geometry_msgs::msg::Pose & stop_pose, - const std::vector stop_factor_points = {}) + const std::vector & stop_factor_points = {}) { StopFactor stop_factor; stop_factor.stop_factor_points = stop_factor_points; @@ -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,19 +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 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 @@ -461,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] = @@ -598,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); }(); @@ -613,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); }(); @@ -829,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}; @@ -850,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); @@ -867,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 @@ -884,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 @@ -926,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 {}; } @@ -963,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/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md index 6ba8f8b929146..318758220c239 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/README.md @@ -508,7 +508,7 @@ If you want to care the occlusion nearby ego more cautiously, set `occlusion.occ #### occupancy grid map tuning -Refer to the document of [probabilistic_occupancy_grid_map](https://autowarefoundation.github.io/autoware.universe/main/perception/probabilistic_occupancy_grid_map/) for details. If occlusion tends to be detected at apparently free space, increase `occlusion.free_space_max` to ignore them. +Refer to the document of [autoware_probabilistic_occupancy_grid_map](https://autowarefoundation.github.io/autoware.universe/main/perception/autoware_probabilistic_occupancy_grid_map/) for details. If occlusion tends to be detected at apparently free space, increase `occlusion.free_space_max` to ignore them. #### in simple_planning_simulator diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 93209a10180be..4a55ba5c1c666 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -35,7 +35,7 @@ std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) } diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) + const std::string & stop_reason, const geometry_msgs::msg::Pose & stop_pose) { diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 5900bd3531266..6815ca3ff8cb4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -76,7 +76,7 @@ bool smoothPath( TrajectoryPoints traj_smoothed; clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; return false; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 330a3443b4e5d..826bdf5a8ed91 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner namespace { RunOutDebug::TextWithPosition createDebugText( - const std::string text, const geometry_msgs::msg::Pose pose, const float lateral_offset) + const std::string & text, const geometry_msgs::msg::Pose pose, const float lateral_offset) { const auto offset_pose = autoware::universe_utils::calcOffsetPose(pose, 0, lateral_offset, 0); @@ -46,7 +46,7 @@ RunOutDebug::TextWithPosition createDebugText( visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const std::vector> & poly, const rclcpp::Time & time, - const std::string ns, const geometry_msgs::msg::Vector3 & scale, + const std::string & ns, const geometry_msgs::msg::Vector3 & scale, const std_msgs::msg::ColorRGBA & color, const double height) { visualization_msgs::msg::MarkerArray marker_array; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index acb862f0eda70..0a38e8d47ea36 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -420,7 +420,7 @@ std::vector RunOutModule::excludeObstaclesOnEgoPath( std::vector RunOutModule::checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time, + const std::vector & poly, const float travel_time, const std::vector> & crosswalk_lanelets) const { const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); @@ -976,7 +976,7 @@ std::vector RunOutModule::excludeObstaclesOutSideOfPartition( } void RunOutModule::publishDebugValue( - const PathWithLaneId & path, const std::vector extracted_obstacles, + const PathWithLaneId & path, const std::vector & extracted_obstacles, const std::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 1922cba9257ab..7d8513485ac52 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -87,7 +87,7 @@ class RunOutModule : public SceneModuleInterface std::vector checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time, + const std::vector & poly, const float travel_time, const std::vector> & crosswalk_lanelets) const; std::optional findNearestCollisionObstacle( @@ -171,7 +171,7 @@ class RunOutModule : public SceneModuleInterface const std::vector & dynamic_obstacles, const PathWithLaneId & path); void publishDebugValue( - const PathWithLaneId & path, const std::vector extracted_obstacles, + const PathWithLaneId & path, const std::vector & extracted_obstacles, const std::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose) const; 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_out_of_lane_module/config/out_of_lane.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml index e57b5a45d8be6..a0cf69ee71027 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -19,6 +19,7 @@ predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights + ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered @@ -27,7 +28,8 @@ action: # action to insert in the trajectory if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed precision: 0.1 # [m] precision when inserting a stop pose in the trajectory - distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle + lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp index 0c793c9f5798e..8b03fa66eab55 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp @@ -70,9 +70,11 @@ std::optional calculate_last_in_lane_pose( std::optional calculate_slowdown_point( const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params) + const std::optional & prev_slowdown_point, PlannerParam params) { - params.extra_front_offset += params.dist_buffer; + params.extra_front_offset += params.lon_dist_buffer; + params.extra_right_offset += params.lat_dist_buffer; + params.extra_left_offset += params.lat_dist_buffer; const auto base_footprint = make_base_footprint(params); // search for the first slowdown decision for which a stop point can be inserted diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index ee4897de86c5b..145f21c94c0d0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -53,6 +53,6 @@ std::optional calculate_last_in_lane_pose( /// @return optional slowdown point to insert in the trajectory std::optional calculate_slowdown_point( const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params); + const std::optional & prev_slowdown_point, PlannerParam params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index ecc10df43c792..1231bf49759d5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -111,6 +111,14 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( }) != object.classification.end(); if (is_pedestrian) continue; + const auto is_coming_from_behind = + motion_utils::calcSignedArcLength( + ego_data.trajectory_points, ego_data.first_trajectory_idx, + object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; + if (params.objects_ignore_behind_ego && is_coming_from_behind) { + continue; + } + auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 8397d0c116090..0afedcd7f9c9a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -36,7 +36,6 @@ #include #include #include -#include #include namespace autoware::motion_velocity_planner @@ -85,6 +84,8 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) pp.objects_dist_buffer = getOrDeclareParameter(node, ns_ + ".objects.distance_buffer"); pp.objects_cut_predicted_paths_beyond_red_lights = getOrDeclareParameter(node, ns_ + ".objects.cut_predicted_paths_beyond_red_lights"); + pp.objects_ignore_behind_ego = + getOrDeclareParameter(node, ns_ + ".objects.ignore_behind_ego"); pp.overlap_min_dist = getOrDeclareParameter(node, ns_ + ".overlap.minimum_distance"); pp.overlap_extra_length = getOrDeclareParameter(node, ns_ + ".overlap.extra_length"); @@ -93,7 +94,9 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) getOrDeclareParameter(node, ns_ + ".action.skip_if_over_max_decel"); pp.precision = getOrDeclareParameter(node, ns_ + ".action.precision"); pp.min_decision_duration = getOrDeclareParameter(node, ns_ + ".action.min_duration"); - pp.dist_buffer = getOrDeclareParameter(node, ns_ + ".action.distance_buffer"); + pp.lon_dist_buffer = + getOrDeclareParameter(node, ns_ + ".action.longitudinal_distance_buffer"); + pp.lat_dist_buffer = getOrDeclareParameter(node, ns_ + ".action.lateral_distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns_ + ".action.slowdown.velocity"); pp.slow_dist_threshold = getOrDeclareParameter(node, ns_ + ".action.slowdown.distance_threshold"); @@ -132,14 +135,15 @@ void OutOfLaneModule::update_parameters(const std::vector & p updateParam( parameters, ns_ + ".objects.cut_predicted_paths_beyond_red_lights", pp.objects_cut_predicted_paths_beyond_red_lights); - + updateParam(parameters, ns_ + ".objects.ignore_behind_ego", pp.objects_ignore_behind_ego); updateParam(parameters, ns_ + ".overlap.minimum_distance", pp.overlap_min_dist); updateParam(parameters, ns_ + ".overlap.extra_length", pp.overlap_extra_length); updateParam(parameters, ns_ + ".action.skip_if_over_max_decel", pp.skip_if_over_max_decel); updateParam(parameters, ns_ + ".action.precision", pp.precision); updateParam(parameters, ns_ + ".action.min_duration", pp.min_decision_duration); - updateParam(parameters, ns_ + ".action.distance_buffer", pp.dist_buffer); + updateParam(parameters, ns_ + ".action.longitudinal_distance_buffer", pp.lon_dist_buffer); + updateParam(parameters, ns_ + ".action.lateral_distance_buffer", pp.lat_dist_buffer); updateParam(parameters, ns_ + ".action.slowdown.velocity", pp.slow_velocity); updateParam(parameters, ns_ + ".action.slowdown.distance_threshold", pp.slow_dist_threshold); updateParam(parameters, ns_ + ".action.stop.distance_threshold", pp.stop_dist_threshold); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index 7a71bb6a7a28a..9c68a0bf23a92 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -56,12 +56,14 @@ struct PlannerParam double objects_min_confidence; // minimum confidence to consider a predicted path double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in // the other lane + bool objects_ignore_behind_ego; // if true, objects behind the ego vehicle are ignored double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap // action to insert in the trajectory if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double dist_buffer; + double lon_dist_buffer; // [m] safety distance buffer to keep in front of the ego vehicle + double lat_dist_buffer; // [m] safety distance buffer to keep on the side of the ego vehicle double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index f181b0c76b51e..8a1e7a1b6ddb5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -361,7 +361,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } traj_smoothed.insert( @@ -418,7 +418,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold); updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold); - set_velocity_smoother_params(); + // set_velocity_smoother_params(); TODO(Maxime): fix update parameters of the velocity smoother rcl_interfaces::msg::SetParametersResult result; result.successful = true; diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index edcb8c2b22061..c44338aa64ce8 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -140,10 +140,10 @@ size_t findEgoSegmentIndex( } std::vector resampleTrajectoryPoints( - const std::vector traj_points, const double interval); + const std::vector & traj_points, const double interval); std::vector resampleTrajectoryPointsWithoutStopPoint( - const std::vector traj_points, const double interval); + const std::vector & traj_points, const double interval); template std::optional updateFrontPointForFix( diff --git a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index c8ca80b95d284..89120eef0f76a 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -59,7 +59,7 @@ void compensateLastPose( } std::vector resampleTrajectoryPoints( - const std::vector traj_points, const double interval) + const std::vector & traj_points, const double interval) { constexpr bool enable_resampling_stop_point = true; @@ -71,7 +71,7 @@ std::vector resampleTrajectoryPoints( // NOTE: stop point will not be resampled std::vector resampleTrajectoryPointsWithoutStopPoint( - const std::vector traj_points, const double interval) + const std::vector & traj_points, const double interval) { constexpr bool enable_resampling_stop_point = false; diff --git a/sensing/image_diagnostics/CMakeLists.txt b/sensing/autoware_image_diagnostics/CMakeLists.txt similarity index 96% rename from sensing/image_diagnostics/CMakeLists.txt rename to sensing/autoware_image_diagnostics/CMakeLists.txt index 186d9340dc08a..6786249639e50 100644 --- a/sensing/image_diagnostics/CMakeLists.txt +++ b/sensing/autoware_image_diagnostics/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(image_diagnostics) +project(autoware_image_diagnostics) ### Compile options if(NOT CMAKE_CXX_STANDARD) diff --git a/sensing/image_diagnostics/README.md b/sensing/autoware_image_diagnostics/README.md similarity index 100% rename from sensing/image_diagnostics/README.md rename to sensing/autoware_image_diagnostics/README.md diff --git a/sensing/image_diagnostics/image/block_state_decision.svg b/sensing/autoware_image_diagnostics/image/block_state_decision.svg similarity index 100% rename from sensing/image_diagnostics/image/block_state_decision.svg rename to sensing/autoware_image_diagnostics/image/block_state_decision.svg diff --git a/sensing/image_diagnostics/image/image_diagnostics_overview.svg b/sensing/autoware_image_diagnostics/image/image_diagnostics_overview.svg similarity index 100% rename from sensing/image_diagnostics/image/image_diagnostics_overview.svg rename to sensing/autoware_image_diagnostics/image/image_diagnostics_overview.svg diff --git a/sensing/image_diagnostics/image/image_status_decision.svg b/sensing/autoware_image_diagnostics/image/image_status_decision.svg similarity index 100% rename from sensing/image_diagnostics/image/image_status_decision.svg rename to sensing/autoware_image_diagnostics/image/image_status_decision.svg diff --git a/sensing/image_diagnostics/launch/image_diagnostics_node.launch.xml b/sensing/autoware_image_diagnostics/launch/image_diagnostics_node.launch.xml similarity index 62% rename from sensing/image_diagnostics/launch/image_diagnostics_node.launch.xml rename to sensing/autoware_image_diagnostics/launch/image_diagnostics_node.launch.xml index 4549ad43c7631..355400d3150da 100644 --- a/sensing/image_diagnostics/launch/image_diagnostics_node.launch.xml +++ b/sensing/autoware_image_diagnostics/launch/image_diagnostics_node.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/sensing/image_diagnostics/package.xml b/sensing/autoware_image_diagnostics/package.xml similarity index 89% rename from sensing/image_diagnostics/package.xml rename to sensing/autoware_image_diagnostics/package.xml index 00d2751b86aa1..37253e1431d3d 100644 --- a/sensing/image_diagnostics/package.xml +++ b/sensing/autoware_image_diagnostics/package.xml @@ -1,9 +1,9 @@ - image_diagnostics + autoware_image_diagnostics 0.1.0 - The image_diagnostics_package + The autoware_image_diagnostics package Dai Nguyen Yoshi Ri Apache License 2.0 diff --git a/sensing/image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp similarity index 96% rename from sensing/image_diagnostics/src/image_diagnostics_node.cpp rename to sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp index 49204f5cbc756..829e0e3ee8d81 100644 --- a/sensing/image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp @@ -50,26 +50,19 @@ void ImageDiagNode::onImageDiagChecker(DiagnosticStatusWrapper & stat) stat.add("number backlight regions ", std::to_string(params_.num_of_regions_backlight)); auto level = DiagnosticStatusWrapper::OK; + std::string msg = "OK"; + if (params_.diagnostic_status < 0) { level = DiagnosticStatusWrapper::STALE; + msg = "STALE"; } else if (params_.diagnostic_status == 1) { level = DiagnosticStatusWrapper::WARN; + msg = "WARNING: abnormal state in image diagnostics"; } else if (params_.diagnostic_status == 2) { level = DiagnosticStatusWrapper::ERROR; - } else { - level = DiagnosticStatusWrapper::OK; - } - - std::string msg; - if (level == DiagnosticStatusWrapper::OK) { - msg = "OK"; - } else if (level == DiagnosticStatusWrapper::WARN) { - msg = "WARNING: abnormal state in image diagnostics"; - } else if (level == DiagnosticStatusWrapper::ERROR) { msg = "ERROR: abnormal state in image diagnostics"; - } else if (level == DiagnosticStatusWrapper::STALE) { - msg = "STALE"; } + stat.summary(level, msg); } @@ -101,7 +94,6 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i img_gray.convertTo(img_gray_32b, CV_32FC1); cv::Mat imgDCT(size, CV_32FC1); imgDCT.setTo(0.0); - float region_freq_average = 0.0; cv::Mat imgDFT(size, CV_32FC1); imgDFT.setTo(0.0); // calculate the features of each small block in image @@ -132,7 +124,7 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i cv::Mat rect_tmp = img_gray_32b(roi); channelImg[0](original_roi).copyTo(rect_tmp); cv::log(rect_tmp, rect_tmp); - region_freq_average = cv::mean(rect_tmp)[0]; + const float region_freq_average = cv::mean(rect_tmp)[0]; region_average_vec.push_back(intensity_average); region_blockage_ratio_vec.push_back(roi_blockage_ratio); diff --git a/sensing/image_diagnostics/src/image_diagnostics_node.hpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp similarity index 100% rename from sensing/image_diagnostics/src/image_diagnostics_node.hpp rename to sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp diff --git a/sensing/image_transport_decompressor/CMakeLists.txt b/sensing/autoware_image_transport_decompressor/CMakeLists.txt similarity index 56% rename from sensing/image_transport_decompressor/CMakeLists.txt rename to sensing/autoware_image_transport_decompressor/CMakeLists.txt index 2ecd3446290df..d658d28fbf68f 100644 --- a/sensing/image_transport_decompressor/CMakeLists.txt +++ b/sensing/autoware_image_transport_decompressor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(image_transport_decompressor) +project(autoware_image_transport_decompressor) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,16 +9,16 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) -ament_auto_add_library(image_transport_decompressor SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/image_transport_decompressor.cpp ) -target_link_libraries(image_transport_decompressor +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ) -rclcpp_components_register_node(image_transport_decompressor - PLUGIN "image_preprocessor::ImageTransportDecompressor" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::image_preprocessor::ImageTransportDecompressor" EXECUTABLE image_transport_decompressor_node ) diff --git a/sensing/image_transport_decompressor/README.md b/sensing/autoware_image_transport_decompressor/README.md similarity index 88% rename from sensing/image_transport_decompressor/README.md rename to sensing/autoware_image_transport_decompressor/README.md index 97e0178eb00d6..acad3739d8fa2 100644 --- a/sensing/image_transport_decompressor/README.md +++ b/sensing/autoware_image_transport_decompressor/README.md @@ -22,7 +22,7 @@ The `image_transport_decompressor` is a node that decompresses images. ## Parameters -{{ json_to_markdown("sensing/image_transport_decompressor/schema/image_transport_decompressor.schema.json") }} +{{ json_to_markdown("sensing/autoware_image_transport_decompressor/schema/image_transport_decompressor.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/image_transport_decompressor/config/image_transport_decompressor.param.yaml b/sensing/autoware_image_transport_decompressor/config/image_transport_decompressor.param.yaml similarity index 100% rename from sensing/image_transport_decompressor/config/image_transport_decompressor.param.yaml rename to sensing/autoware_image_transport_decompressor/config/image_transport_decompressor.param.yaml diff --git a/sensing/image_transport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp b/sensing/autoware_image_transport_decompressor/include/autoware/image_transport_decompressor/image_transport_decompressor.hpp similarity index 78% rename from sensing/image_transport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp rename to sensing/autoware_image_transport_decompressor/include/autoware/image_transport_decompressor/image_transport_decompressor.hpp index d192429dd150e..b3d343882a880 100644 --- a/sensing/image_transport_decompressor/include/image_transport_decompressor/image_transport_decompressor.hpp +++ b/sensing/autoware_image_transport_decompressor/include/autoware/image_transport_decompressor/image_transport_decompressor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ -#define IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ +#ifndef AUTOWARE__IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ +#define AUTOWARE__IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ #include @@ -24,7 +24,7 @@ #include #include -namespace image_preprocessor +namespace autoware::image_preprocessor { class ImageTransportDecompressor : public rclcpp::Node { @@ -40,6 +40,6 @@ class ImageTransportDecompressor : public rclcpp::Node std::string encoding_; }; -} // namespace image_preprocessor +} // namespace autoware::image_preprocessor -#endif // IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ +#endif // AUTOWARE__IMAGE_TRANSPORT_DECOMPRESSOR__IMAGE_TRANSPORT_DECOMPRESSOR_HPP_ diff --git a/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml b/sensing/autoware_image_transport_decompressor/launch/image_transport_decompressor.launch.xml similarity index 55% rename from sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml rename to sensing/autoware_image_transport_decompressor/launch/image_transport_decompressor.launch.xml index 61296531f17f2..e427e0c8b1121 100644 --- a/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml +++ b/sensing/autoware_image_transport_decompressor/launch/image_transport_decompressor.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/sensing/image_transport_decompressor/package.xml b/sensing/autoware_image_transport_decompressor/package.xml similarity index 95% rename from sensing/image_transport_decompressor/package.xml rename to sensing/autoware_image_transport_decompressor/package.xml index ed7ae7ae060b5..e4e8d69acb4d0 100644 --- a/sensing/image_transport_decompressor/package.xml +++ b/sensing/autoware_image_transport_decompressor/package.xml @@ -1,7 +1,7 @@ - image_transport_decompressor + autoware_image_transport_decompressor 0.0.0 The image_transport_decompressor package Yukihiro Saito diff --git a/sensing/image_transport_decompressor/schema/image_transport_decompressor.schema.json b/sensing/autoware_image_transport_decompressor/schema/image_transport_decompressor.schema.json similarity index 83% rename from sensing/image_transport_decompressor/schema/image_transport_decompressor.schema.json rename to sensing/autoware_image_transport_decompressor/schema/image_transport_decompressor.schema.json index fdbe1bcd721dd..508517dd7bac2 100644 --- a/sensing/image_transport_decompressor/schema/image_transport_decompressor.schema.json +++ b/sensing/autoware_image_transport_decompressor/schema/image_transport_decompressor.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Image Transport Decompressor", "type": "object", "definitions": { - "image_transport_decompressor": { + "autoware_image_transport_decompressor": { "type": "object", "properties": { "encoding": { @@ -20,7 +20,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/image_transport_decompressor" + "$ref": "#/definitions/autoware_image_transport_decompressor" } }, "required": ["ros__parameters"] diff --git a/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp b/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp similarity index 96% rename from sensing/image_transport_decompressor/src/image_transport_decompressor.cpp rename to sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp index ffc1e5865c856..b1971f892a352 100644 --- a/sensing/image_transport_decompressor/src/image_transport_decompressor.cpp +++ b/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp @@ -46,7 +46,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "image_transport_decompressor/image_transport_decompressor.hpp" +#include "autoware/image_transport_decompressor/image_transport_decompressor.hpp" #include #include @@ -65,7 +65,7 @@ #include #include -namespace image_preprocessor +namespace autoware::image_preprocessor { ImageTransportDecompressor::ImageTransportDecompressor(const rclcpp::NodeOptions & node_options) : rclcpp::Node("image_transport_decompressor", node_options), @@ -179,7 +179,7 @@ void ImageTransportDecompressor::onCompressedImage( raw_image_pub_->publish(std::move(image_ptr)); } } -} // namespace image_preprocessor +} // namespace autoware::image_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(image_preprocessor::ImageTransportDecompressor) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::image_preprocessor::ImageTransportDecompressor) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt similarity index 81% rename from sensing/pointcloud_preprocessor/CMakeLists.txt rename to sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 76b9cc91ea586..744bac480e058 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(pointcloud_preprocessor) +project(autoware_pointcloud_preprocessor) find_package(autoware_cmake REQUIRED) autoware_package() @@ -98,99 +98,99 @@ target_link_libraries(pointcloud_preprocessor_filter # ========== Time synchronizer ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PointCloudDataSynchronizerComponent" + PLUGIN "autoware::pointcloud_preprocessor::PointCloudDataSynchronizerComponent" EXECUTABLE time_synchronizer_node) # ========== Concatenate data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PointCloudConcatenationComponent" + PLUGIN "autoware::pointcloud_preprocessor::PointCloudConcatenationComponent" EXECUTABLE concatenate_pointclouds_node) # ========== Concatenate and Sync data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" + PLUGIN "autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" EXECUTABLE concatenate_data_node) # ========== CropBox ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::CropBoxFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::CropBoxFilterComponent" EXECUTABLE crop_box_filter_node) # ========== Down Sampler Filter ========== # -- Voxel Grid Downsample Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::VoxelGridDownsampleFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::VoxelGridDownsampleFilterComponent" EXECUTABLE voxel_grid_downsample_filter_node) # -- Pickup Based Voxel Grid Downsample Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" EXECUTABLE pickup_based_voxel_grid_downsample_filter_node) # -- Random Downsample Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::RandomDownsampleFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::RandomDownsampleFilterComponent" EXECUTABLE random_downsample_filter_node) rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::ApproximateDownsampleFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::ApproximateDownsampleFilterComponent" EXECUTABLE approximate_downsample_filter_node) # ========== Outlier Filter ========== # -- Ring Outlier Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::RingOutlierFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::RingOutlierFilterComponent" EXECUTABLE ring_outlier_filter_node) # -- Voxel Grid Outlier Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::VoxelGridOutlierFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::VoxelGridOutlierFilterComponent" EXECUTABLE voxel_grid_outlier_filter_node) # -- Radius Search 2D Outlier Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::RadiusSearch2DOutlierFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::RadiusSearch2DOutlierFilterComponent" EXECUTABLE radius_search_2d_outlier_filter_node) # -- DualReturn Outlier Filter-- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::DualReturnOutlierFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::DualReturnOutlierFilterComponent" EXECUTABLE dual_return_outlier_filter_node) # ========== Passthrough Filter ========== # -- Passthrough Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PassThroughFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::PassThroughFilterComponent" EXECUTABLE passthrough_filter_node) # -- Passthrough Filter Uint16 -- rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PassThroughFilterUInt16Component" + PLUGIN "autoware::pointcloud_preprocessor::PassThroughFilterUInt16Component" EXECUTABLE passthrough_filter_uint16_node) # ========== Pointcloud Accumulator Filter ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PointcloudAccumulatorComponent" + PLUGIN "autoware::pointcloud_preprocessor::PointcloudAccumulatorComponent" EXECUTABLE pointcloud_accumulator_node) # ========== Vector Map Filter ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::Lanelet2MapFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::Lanelet2MapFilterComponent" EXECUTABLE vector_map_filter_node) # ========== Distortion Corrector ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::DistortionCorrectorComponent" + PLUGIN "autoware::pointcloud_preprocessor::DistortionCorrectorComponent" EXECUTABLE distortion_corrector_node) # ========== Blockage Diagnostics =========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::BlockageDiagComponent" + PLUGIN "autoware::pointcloud_preprocessor::BlockageDiagComponent" EXECUTABLE blockage_diag_node) # ========== PolygonRemover ========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::PolygonRemoverComponent" + PLUGIN "autoware::pointcloud_preprocessor::PolygonRemoverComponent" EXECUTABLE polygon_remover_node) set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE) @@ -198,7 +198,7 @@ target_link_libraries(polygon_remover_node gmp CGAL CGAL::CGAL CGAL::CGAL_Core) # ========== Vector Map Inside Area Filter =========== rclcpp_components_register_node(pointcloud_preprocessor_filter - PLUGIN "pointcloud_preprocessor::VectorMapInsideAreaFilterComponent" + PLUGIN "autoware::pointcloud_preprocessor::VectorMapInsideAreaFilterComponent" EXECUTABLE vector_map_inside_area_filter_node) install( diff --git a/sensing/pointcloud_preprocessor/README.md b/sensing/autoware_pointcloud_preprocessor/README.md similarity index 96% rename from sensing/pointcloud_preprocessor/README.md rename to sensing/autoware_pointcloud_preprocessor/README.md index 5c6402efdf23d..4424259a52daa 100644 --- a/sensing/pointcloud_preprocessor/README.md +++ b/sensing/autoware_pointcloud_preprocessor/README.md @@ -1,8 +1,8 @@ -# pointcloud_preprocessor +# autoware_pointcloud_preprocessor ## Purpose -The `pointcloud_preprocessor` is a package that includes the following filters: +The `autoware_pointcloud_preprocessor` is a package that includes the following filters: - removing outlier points - cropping @@ -56,7 +56,7 @@ Detail description of each filter's algorithm is in the following links. ## Assumptions / Known limits -`pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because +`autoware::pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). ## Measuring the performance diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml similarity index 100% rename from sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml rename to sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml diff --git a/sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml similarity index 100% rename from sensing/pointcloud_preprocessor/config/distortion_corrector_node.param.yaml rename to sensing/autoware_pointcloud_preprocessor/config/distortion_corrector_node.param.yaml diff --git a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml b/sensing/autoware_pointcloud_preprocessor/config/processing_time_ms.xml similarity index 100% rename from sensing/pointcloud_preprocessor/config/processing_time_ms.xml rename to sensing/autoware_pointcloud_preprocessor/config/processing_time_ms.xml diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md similarity index 98% rename from sensing/pointcloud_preprocessor/docs/blockage_diag.md rename to sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md index 082ae180fa3c9..b6f858ada3a98 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md @@ -28,7 +28,7 @@ The area of noise is found by erosion and dilation these black pixels. ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Input diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md similarity index 100% rename from sensing/pointcloud_preprocessor/docs/concatenate-data.md rename to sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md diff --git a/sensing/pointcloud_preprocessor/docs/crop-box-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/crop-box-filter.md similarity index 84% rename from sensing/pointcloud_preprocessor/docs/crop-box-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/crop-box-filter.md index b6bd960c99919..4539a85e26a1d 100644 --- a/sensing/pointcloud_preprocessor/docs/crop-box-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/crop-box-filter.md @@ -10,13 +10,13 @@ The `crop_box_filter` is a node that removes points with in a given box region. ## Inputs / Outputs -This implementation inherit `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherit `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Node Parameters -This implementation inherit `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherit `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Core Parameters diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md similarity index 93% rename from sensing/pointcloud_preprocessor/docs/distortion-corrector.md rename to sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md index 033e82607057a..ab5a07b5279bc 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md @@ -36,12 +36,12 @@ Please note that the processing time difference between the two distortion metho ### Core Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json") }} +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json") }} ## Launch ```bash -ros2 launch pointcloud_preprocessor distortion_corrector.launch.xml +ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml ``` ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md similarity index 91% rename from sensing/pointcloud_preprocessor/docs/downsample-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index 25faf62b765bc..ac721171cb014 100644 --- a/sensing/pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -24,13 +24,13 @@ This algorithm samples a single actual point existing within the voxel, not the ## Inputs / Outputs -These implementations inherit `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Note Parameters -These implementations inherit `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Core Parameters diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md similarity index 96% rename from sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index d5993a803fa87..6c9a7ed14c2eb 100644 --- a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -32,7 +32,7 @@ The below picture shows the ROI options. ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Output @@ -46,7 +46,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Node Parameters -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Core Parameters diff --git a/sensing/pointcloud_preprocessor/docs/image/blockage_diag.png b/sensing/autoware_pointcloud_preprocessor/docs/image/blockage_diag.png similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/blockage_diag.png rename to sensing/autoware_pointcloud_preprocessor/docs/image/blockage_diag.png diff --git a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/distortion_corrector.jpg b/sensing/autoware_pointcloud_preprocessor/docs/image/distortion_corrector.jpg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/distortion_corrector.jpg rename to sensing/autoware_pointcloud_preprocessor/docs/image/distortion_corrector.jpg diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png b/sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png rename to sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_overall.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-dual_return_overall.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_overall.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-dual_return_overall.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-radius_search_2d.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-radius_search_2d.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/outlier_filter-radius_search_2d.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-radius_search_2d.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-return_type.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-return_type.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/outlier_filter-return_type.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-return_type.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-ring.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-ring.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/outlier_filter-ring.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-ring.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-voxel_grid.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-voxel_grid.drawio.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/outlier_filter-voxel_grid.drawio.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/outlier_filter-voxel_grid.drawio.svg diff --git a/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png b/sensing/autoware_pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png rename to sensing/autoware_pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png diff --git a/sensing/pointcloud_preprocessor/docs/image/vector_map_inside_area_filter_overview.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/vector_map_inside_area_filter_overview.svg similarity index 100% rename from sensing/pointcloud_preprocessor/docs/image/vector_map_inside_area_filter_overview.svg rename to sensing/autoware_pointcloud_preprocessor/docs/image/vector_map_inside_area_filter_overview.svg diff --git a/sensing/pointcloud_preprocessor/docs/outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/outlier-filter.md similarity index 100% rename from sensing/pointcloud_preprocessor/docs/outlier-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/outlier-filter.md diff --git a/sensing/pointcloud_preprocessor/docs/passthrough-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md similarity index 100% rename from sensing/pointcloud_preprocessor/docs/passthrough-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md diff --git a/sensing/pointcloud_preprocessor/docs/pointcloud-accumulator.md b/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-accumulator.md similarity index 100% rename from sensing/pointcloud_preprocessor/docs/pointcloud-accumulator.md rename to sensing/autoware_pointcloud_preprocessor/docs/pointcloud-accumulator.md diff --git a/sensing/pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md similarity index 88% rename from sensing/pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md index 19d5c348afff5..d0d621b586da6 100644 --- a/sensing/pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/radius-search-2d-outlier-filter.md @@ -14,13 +14,13 @@ The description above is quoted from [1]. `pcl::search::KdTree` [2] is used to i ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Node Parameters -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Core Parameters diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md similarity index 95% rename from sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md index 0a179ddf6a8ef..7f2efe90a674f 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -46,13 +46,13 @@ stop ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Node Parameters -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Core Parameters diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md similarity index 100% rename from sensing/pointcloud_preprocessor/docs/vector-map-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-inside-area-filter.md similarity index 93% rename from sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/vector-map-inside-area-filter.md index 8e31dfb203d2b..1b6f86a7e4f27 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-inside-area-filter.md @@ -16,7 +16,7 @@ The `vector_map_inside_area_filter` is a node that removes points inside the vec ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, so please see also [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, so please see also [README](../README.md). ### Input diff --git a/sensing/pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md similarity index 85% rename from sensing/pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md rename to sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md index 0c1905e810fec..d99b54ef09a73 100644 --- a/sensing/pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md @@ -13,13 +13,13 @@ The [radius_search_2d_outlier_filter](./radius-search-2d-outlier-filter.md) is b ## Inputs / Outputs -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ## Parameters ### Node Parameters -This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). +This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). ### Core Parameters diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp similarity index 88% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index dd24c83761878..18f6c3851866d 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include @@ -38,12 +38,12 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; -class BlockageDiagComponent : public pointcloud_preprocessor::Filter +class BlockageDiagComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -103,6 +103,6 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter explicit BlockageDiagComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp similarity index 94% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 619a0b8520b97..2443e45fe4c63 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -49,8 +49,8 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ #include #include @@ -85,7 +85,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; @@ -189,6 +189,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp similarity index 94% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index 77717c51e6981..36b1910a4d798 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -49,8 +49,8 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ -#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ #include #include @@ -84,7 +84,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; @@ -176,6 +176,6 @@ class PointCloudConcatenationComponent : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp similarity index 86% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp index 1406679ce0dba..09b5c7fd5f5fa 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -50,11 +50,11 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" #include @@ -62,9 +62,9 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class CropBoxFilterComponent : public pointcloud_preprocessor::Filter +class CropBoxFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -102,6 +102,6 @@ class CropBoxFilterComponent : public pointcloud_preprocessor::Filter PCL_MAKE_ALIGNED_OPERATOR_NEW explicit CropBoxFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp similarity index 94% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index e37329a9a4cc3..8226cb6eb774c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ -#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #include #include @@ -42,7 +42,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { class DistortionCorrectorBase @@ -170,6 +170,6 @@ class DistortionCorrector3D : public DistortionCorrector const std::string & base_frame, const std::string & lidar_frame) override; }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp similarity index 81% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp index 162170b193827..0d8c9436bda4b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ -#define POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" +#include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" #include #include @@ -29,7 +29,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using rcl_interfaces::msg::SetParametersResult; using sensor_msgs::msg::PointCloud2; @@ -60,6 +60,6 @@ class DistortionCorrectorComponent : public rclcpp::Node void onImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_NODE_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp similarity index 82% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp index 70dcdcab1c2e6..d5d2750442a2f 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp @@ -48,10 +48,10 @@ * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ -#ifndef POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include @@ -60,9 +60,9 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class ApproximateDownsampleFilterComponent : public pointcloud_preprocessor::Filter +class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: void filter( @@ -80,6 +80,8 @@ class ApproximateDownsampleFilterComponent : public pointcloud_preprocessor::Fil PCL_MAKE_ALIGNED_OPERATOR_NEW explicit ApproximateDownsampleFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp similarity index 94% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp index 379c4c79e555a..89d6ea51a7283 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp @@ -14,7 +14,7 @@ #pragma once -#include "pointcloud_preprocessor/transform_info.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { class FasterVoxelGridDownsampleFilter @@ -96,4 +96,4 @@ class FasterVoxelGridDownsampleFilter const TransformInfo & transform_info); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp similarity index 76% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp index a923473ecbe36..9a1f2780be11b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ -#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ // NOLINT -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { /** * @class PickupBasedVoxelGridDownsampleFilterComponent @@ -36,7 +36,8 @@ namespace pointcloud_preprocessor * and picking a representative point for each voxel. It's useful for reducing computational * load when processing large point clouds. */ -class PickupBasedVoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filter +class PickupBasedVoxelGridDownsampleFilterComponent +: public autoware::pointcloud_preprocessor::Filter { protected: void filter( @@ -57,8 +58,8 @@ class PickupBasedVoxelGridDownsampleFilterComponent : public pointcloud_preproce PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PickupBasedVoxelGridDownsampleFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__PICKUP_BASED_VOXEL_GRID_DOWNSAMPLE_FILTER_HPP_ // NOLINT // clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp similarity index 83% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp index c5a30fba37156..f820793fb86f8 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp @@ -48,18 +48,18 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class RandomDownsampleFilterComponent : public pointcloud_preprocessor::Filter +class RandomDownsampleFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: void filter( @@ -78,6 +78,8 @@ class RandomDownsampleFilterComponent : public pointcloud_preprocessor::Filter PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RandomDownsampleFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__RANDOM_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp similarity index 82% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp index ae285b4fa9239..1af2eb5b04552 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp @@ -48,20 +48,20 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" #include #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filter +class VoxelGridDownsampleFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: void filter( @@ -88,6 +88,8 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelGridDownsampleFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp similarity index 97% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp index ce003f58dcc49..a6e113412231a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp @@ -49,10 +49,10 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_ -#define POINTCLOUD_PREPROCESSOR__FILTER_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__FILTER_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__FILTER_HPP_ -#include "pointcloud_preprocessor/transform_info.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" #include #include @@ -84,7 +84,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { namespace sync_policies = message_filters::sync_policies; @@ -302,6 +302,6 @@ class Filter : public rclcpp::Node void setupTF(); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__FILTER_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__FILTER_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp similarity index 82% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp index 940b24ae30569..b8aba769b17a5 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include @@ -38,12 +38,12 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; -class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter +class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -91,6 +91,8 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter explicit DualReturnOutlierFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp similarity index 71% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp index 19c36a1bdb1ce..ffb737b0b9b06 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ // NOLINT -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include @@ -26,9 +26,9 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class RadiusSearch2DOutlierFilterComponent : public pointcloud_preprocessor::Filter +class RadiusSearch2DOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -52,6 +52,8 @@ class RadiusSearch2DOutlierFilterComponent : public pointcloud_preprocessor::Fil PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RadiusSearch2DOutlierFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RADIUS_SEARCH_2D_OUTLIER_FILTER_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp similarity index 85% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index 08fbfe73f2744..caedeac62b88a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" #include "autoware_point_types/types.hpp" -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" #include #include @@ -33,11 +33,11 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using point_cloud_msg_wrapper::PointCloud2Modifier; -class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter +class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: using InputPointIndex = autoware_point_types::PointXYZIRCAEDTIndex; @@ -107,5 +107,5 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +} // namespace autoware::pointcloud_preprocessor +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp similarity index 71% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp index f312b6c0b8505..4ec50c53c2393 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class VoxelGridOutlierFilterComponent : public pointcloud_preprocessor::Filter +class VoxelGridOutlierFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -48,6 +48,6 @@ class VoxelGridOutlierFilterComponent : public pointcloud_preprocessor::Filter PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VoxelGridOutlierFilterComponent(const rclcpp::NodeOptions & option); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp similarity index 84% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp index 5fafa7383b54e..bbb2c0b113658 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp @@ -48,18 +48,18 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class PassThroughFilterComponent : public pointcloud_preprocessor::Filter +class PassThroughFilterComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -75,6 +75,6 @@ class PassThroughFilterComponent : public pointcloud_preprocessor::Filter PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PassThroughFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp similarity index 63% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp index 678da0fbe6b86..0ffb50dc65092 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ // NOLINT -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp" #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class PassThroughFilterUInt16Component : public pointcloud_preprocessor::Filter +class PassThroughFilterUInt16Component : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -43,6 +43,8 @@ class PassThroughFilterUInt16Component : public pointcloud_preprocessor::Filter PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PassThroughFilterUInt16Component(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_FILTER_UINT16_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp similarity index 98% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp index 8115a46c1ffbc..7da891f20955b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp @@ -50,8 +50,8 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ -#define POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ #include #include @@ -471,4 +471,4 @@ void pcl::PassThroughUInt16::applyFilterIndices(std::vector & indic #include #endif -#endif // POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__PASSTHROUGH_FILTER__PASSTHROUGH_UINT16_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp similarity index 67% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp index ca4f0d6ccd2be..6646426a29b99 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ // NOLINT -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class PointcloudAccumulatorComponent : public pointcloud_preprocessor::Filter +class PointcloudAccumulatorComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -43,6 +43,8 @@ class PointcloudAccumulatorComponent : public pointcloud_preprocessor::Filter PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PointcloudAccumulatorComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ +// clang-format off +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_ACCUMULATOR__POINTCLOUD_ACCUMULATOR_NODELET_HPP_ // NOLINT +// clang-format on diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp similarity index 78% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp index 4d2e66eea700e..3054a4454fb00 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ -#define POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/geometry.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/utility/geometry.hpp" #include #include @@ -27,9 +27,9 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class PolygonRemoverComponent : public pointcloud_preprocessor::Filter +class PolygonRemoverComponent : public autoware::pointcloud_preprocessor::Filter { protected: virtual void filter( @@ -60,6 +60,6 @@ class PolygonRemoverComponent : public pointcloud_preprocessor::Filter PCL_MAKE_ALIGNED_OPERATOR_NEW explicit PolygonRemoverComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__POLYGON_REMOVER__POLYGON_REMOVER_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp similarity index 95% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 2dc98a571ff2c..00704b20b85ef 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -49,8 +49,8 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ #include #include @@ -85,7 +85,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using autoware_point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; @@ -186,6 +186,6 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node std::unique_ptr debug_publisher_; }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/transform_info.hpp similarity index 91% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/transform_info.hpp index a4806555695b5..9c4ba7c5ea7ea 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/transform_info.hpp @@ -16,7 +16,7 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { /** @@ -40,4 +40,4 @@ struct TransformInfo bool need_transform; }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/geometry.hpp similarity index 90% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/geometry.hpp index 0175f88ceb89c..e4aacb1a80d6c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/geometry.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/geometry.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ -#define POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ #include #include @@ -33,7 +33,7 @@ using K = CGAL::Exact_predicates_inexact_constructions_kernel; using PointCgal = K::Point_2; using PolygonCgal = std::vector; -namespace pointcloud_preprocessor::utils +namespace autoware::pointcloud_preprocessor::utils { /** * @brief convert ROS polygon to CGAL polygon @@ -81,6 +81,6 @@ void remove_polygon_cgal_from_cloud( bool point_within_cgal_polys( const pcl::PointXYZ & point, const std::vector & polyline_polygons); -} // namespace pointcloud_preprocessor::utils +} // namespace autoware::pointcloud_preprocessor::utils -#endif // POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__GEOMETRY_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp similarity index 85% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp index ef87a4f31457b..4810c5ce84130 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/memory.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ -#define POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ #include -namespace pointcloud_preprocessor::utils +namespace autoware::pointcloud_preprocessor::utils { /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ @@ -35,6 +35,6 @@ bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::Poin * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */ bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input); -} // namespace pointcloud_preprocessor::utils +} // namespace autoware::pointcloud_preprocessor::utils -#endif // POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__UTILITY__MEMORY_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp similarity index 89% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index 2bc9e70c8e29b..4ba773ed618ac 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ #include #include @@ -44,7 +44,7 @@ using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::MultiPoint2d; using autoware::universe_utils::Point2d; -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { class Lanelet2MapFilterComponent : public rclcpp::Node { @@ -95,6 +95,6 @@ class Lanelet2MapFilterComponent : public rclcpp::Node rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp similarity index 70% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index d28a6550123cf..a6b497b40cd77 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ -#define POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ -#include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/geometry.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/utility/geometry.hpp" #include #include @@ -28,9 +28,9 @@ using autoware::universe_utils::MultiPoint2d; -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { -class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filter +class VectorMapInsideAreaFilterComponent : public autoware::pointcloud_preprocessor::Filter { private: void filter( @@ -51,6 +51,6 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte explicit VectorMapInsideAreaFilterComponent(const rclcpp::NodeOptions & options); }; -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_ diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag.launch.xml similarity index 82% rename from sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/blockage_diag.launch.xml index 5a93cb950b827..281ddd7ce434e 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -5,8 +5,8 @@ - - + + diff --git a/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml similarity index 71% rename from sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml index d970aae102264..bde9ca9e59ddd 100644 --- a/sensing/pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/distortion_corrector_node.launch.xml @@ -5,8 +5,8 @@ - - + + diff --git a/sensing/pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml similarity index 87% rename from sensing/pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml index db51e9bafc90e..c65996fbcdc65 100644 --- a/sensing/pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py b/sensing/autoware_pointcloud_preprocessor/launch/polygon_remover.launch.py similarity index 93% rename from sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py rename to sensing/autoware_pointcloud_preprocessor/launch/polygon_remover.launch.py index 98bc5a6dd2699..801642bac79ac 100644 --- a/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py +++ b/sensing/autoware_pointcloud_preprocessor/launch/polygon_remover.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): ns = "pointcloud_preprocessor" - pkg = "pointcloud_preprocessor" + pkg = "autoware_pointcloud_preprocessor" param_file = os.path.join( get_package_share_directory("autoware_vehicle_info_utils"), "config/polygon_remover.yaml" @@ -34,7 +34,7 @@ def generate_launch_description(): my_component = ComposableNode( package=pkg, - plugin="pointcloud_preprocessor::PolygonRemoverComponent", + plugin="autoware::pointcloud_preprocessor::PolygonRemoverComponent", name="polygon_remover", parameters=[ { diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/autoware_pointcloud_preprocessor/launch/preprocessor.launch.py similarity index 93% rename from sensing/pointcloud_preprocessor/launch/preprocessor.launch.py rename to sensing/autoware_pointcloud_preprocessor/launch/preprocessor.launch.py index d82c23152aee4..108cf6d4c4213 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/autoware_pointcloud_preprocessor/launch/preprocessor.launch.py @@ -24,7 +24,7 @@ def launch_setup(context, *args, **kwargs): ns = "pointcloud_preprocessor" - pkg = "pointcloud_preprocessor" + pkg = "autoware_pointcloud_preprocessor" separate_concatenate_node_and_time_sync_node = LaunchConfiguration( "separate_concatenate_node_and_time_sync_node" @@ -36,7 +36,7 @@ def launch_setup(context, *args, **kwargs): if not is_separate_concatenate_node_and_time_sync_node: sync_and_concat_component = ComposableNode( package=pkg, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + plugin="autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="sync_and_concatenate_filter", remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), @@ -56,7 +56,7 @@ def launch_setup(context, *args, **kwargs): else: time_sync_component = ComposableNode( package=pkg, - plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent", + plugin="autoware::pointcloud_preprocessor::PointCloudDataSynchronizerComponent", name="synchronizer_filter", remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), @@ -73,7 +73,7 @@ def launch_setup(context, *args, **kwargs): concat_component = ComposableNode( package=pkg, - plugin="pointcloud_preprocessor::PointCloudConcatenationComponent", + plugin="autoware::pointcloud_preprocessor::PointCloudConcatenationComponent", name="concatenate_filter", remappings=[("output", "points_raw/concatenated")], parameters=[ @@ -89,7 +89,7 @@ def launch_setup(context, *args, **kwargs): # set crop box filter as a component cropbox_component = ComposableNode( package=pkg, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name="crop_box_filter", remappings=[ ( diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/preprocessor.launch.xml similarity index 88% rename from sensing/pointcloud_preprocessor/launch/preprocessor.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/preprocessor.launch.xml index bba1a1b1c6a8c..81add45a62684 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/preprocessor.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/sensing/pointcloud_preprocessor/launch/random_downsample_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter.launch.xml similarity index 84% rename from sensing/pointcloud_preprocessor/launch/random_downsample_filter.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter.launch.xml index edd4f6e92aa8d..c9d0bc3ce0608 100644 --- a/sensing/pointcloud_preprocessor/launch/random_downsample_filter.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/random_downsample_filter.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/sensing/pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml similarity index 90% rename from sensing/pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml rename to sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml index c121a7f0af6ac..a9ed23e9faa3a 100644 --- a/sensing/pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_passthrough_filter.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml similarity index 94% rename from sensing/pointcloud_preprocessor/package.xml rename to sensing/autoware_pointcloud_preprocessor/package.xml index 3eea0f3e2eb9a..557e835a247e9 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -1,9 +1,9 @@ - pointcloud_preprocessor + autoware_pointcloud_preprocessor 0.1.0 - The ROS 2 pointcloud_preprocessor package + The ROS 2 autoware_pointcloud_preprocessor package amc-nu Yukihiro Saito Shunsuke Miura diff --git a/sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json similarity index 100% rename from sensing/pointcloud_preprocessor/schema/distortion_corrector_node.schema.json rename to sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp similarity index 98% rename from sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index aae50a18f2024..9d086e16dd51f 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp" #include "autoware_point_types/types.hpp" #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using autoware_point_types::PointXYZIRCAEDT; using diagnostic_msgs::msg::DiagnosticStatus; @@ -108,7 +108,7 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo] auto level = DiagnosticStatus::OK; - std::string msg; + std::string msg = "OK"; if (ground_blockage_ratio_ < 0) { level = DiagnosticStatus::STALE; msg = "STALE"; @@ -120,9 +120,6 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) } else if (ground_blockage_ratio_ > 0.0f) { level = DiagnosticStatus::WARN; msg = "WARN"; - } else { - level = DiagnosticStatus::OK; - msg = "OK"; } if ((ground_blockage_ratio_ > 0.0f) && (sky_blockage_ratio_ > 0.0f)) { @@ -139,7 +136,7 @@ void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrap { stat.add("ground_dust_ratio", std::to_string(ground_dust_ratio_)); auto level = DiagnosticStatus::OK; - std::string msg; + std::string msg = "OK"; if (ground_dust_ratio_ < 0.0f) { level = DiagnosticStatus::STALE; msg = "STALE"; @@ -150,9 +147,6 @@ void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrap } else if (ground_dust_ratio_ > 0.0f) { level = DiagnosticStatus::WARN; msg = "WARN"; - } else { - level = DiagnosticStatus::OK; - msg = "OK"; } if (ground_dust_ratio_ > 0.0f) { @@ -462,7 +456,7 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( result.reason = "success"; return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::BlockageDiagComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::BlockageDiagComponent) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp similarity index 98% rename from sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index c550c9dfb4933..95c353212484c 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -49,9 +49,9 @@ * */ -#include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" -#include "pointcloud_preprocessor/utility/memory.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" #include @@ -68,7 +68,7 @@ ////////////////////////////////////////////////////////////////////////////////////////////// -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) @@ -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; }); @@ -728,8 +728,8 @@ void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( : "Some topics are not concatenated"; stat.summary(level, message); } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include RCLCPP_COMPONENTS_REGISTER_NODE( - pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent) + autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp similarity index 97% rename from sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp rename to sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index d1f02f5f0bbf1..24c4b572181fe 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp" -#include "pointcloud_preprocessor/utility/memory.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" #include @@ -31,7 +31,7 @@ ////////////////////////////////////////////////////////////////////////////////////////////// -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PointCloudConcatenationComponent::PointCloudConcatenationComponent( const rclcpp::NodeOptions & node_options) @@ -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; }); @@ -504,7 +504,7 @@ void PointCloudConcatenationComponent::checkConcatStatus( : "Some topics are not concatenated"; stat.summary(level, message); } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointCloudConcatenationComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::PointCloudConcatenationComponent) diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp similarity index 97% rename from sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index a56d8f1570234..a09b6bfb6a1ce 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -49,13 +49,13 @@ * */ -#include "pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp" #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options) : Filter("CropBoxFilter", options) @@ -296,7 +296,7 @@ rcl_interfaces::msg::SetParametersResult CropBoxFilterComponent::paramCallback( return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::CropBoxFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::CropBoxFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp similarity index 98% rename from sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp rename to sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 4a7152a3cd38a..652edd9126358 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" +#include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -#include "pointcloud_preprocessor/utility/memory.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { template @@ -463,4 +463,4 @@ inline void DistortionCorrector3D::undistortPointImplementation( prev_transformation_matrix_ = transformation_matrix_; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp similarity index 92% rename from sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp rename to sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index b5cf581301a23..822bb2ba5add5 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp" +#include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector_node.hpp" -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" +#include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { /** @brief Constructor. */ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options) @@ -118,7 +118,7 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr pointclou } } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DistortionCorrectorComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::DistortionCorrectorComponent) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp similarity index 93% rename from sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp index 6a9a5818b19ce..a4ccca9beea72 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp @@ -49,7 +49,7 @@ * */ -#include "pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp" #include #include @@ -57,7 +57,7 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { ApproximateDownsampleFilterComponent::ApproximateDownsampleFilterComponent( const rclcpp::NodeOptions & options) @@ -117,7 +117,8 @@ rcl_interfaces::msg::SetParametersResult ApproximateDownsampleFilterComponent::p return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::ApproximateDownsampleFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::pointcloud_preprocessor::ApproximateDownsampleFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp similarity index 97% rename from sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index b9488d1ed7a73..c0e41239057f0 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { FasterVoxelGridDownsampleFilter::FasterVoxelGridDownsampleFilter() @@ -195,4 +195,4 @@ void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( } } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp similarity index 96% rename from sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp index 83c9058fd5fa5..834f9b8d8fbc0 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/pickup_based_voxel_grid_downsample_filter.hpp" #include "robin_hood.h" @@ -49,7 +49,7 @@ struct VoxelKeyEqual }; } // namespace -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PickupBasedVoxelGridDownsampleFilterComponent::PickupBasedVoxelGridDownsampleFilterComponent( const rclcpp::NodeOptions & options) @@ -186,8 +186,8 @@ PickupBasedVoxelGridDownsampleFilterComponent::paramCallback( return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include RCLCPP_COMPONENTS_REGISTER_NODE( - pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent) + autoware::pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp similarity index 93% rename from sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp index 6216b1dad97e8..32abafb759b73 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/random_downsample_filter_nodelet.cpp @@ -46,11 +46,11 @@ * */ -#include "pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/random_downsample_filter_nodelet.hpp" #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { RandomDownsampleFilterComponent::RandomDownsampleFilterComponent( const rclcpp::NodeOptions & options) @@ -103,6 +103,6 @@ rcl_interfaces::msg::SetParametersResult RandomDownsampleFilterComponent::paramC return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::RandomDownsampleFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::RandomDownsampleFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h similarity index 99% rename from sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h index edbc55c8158c0..f04a84968dbbb 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -2495,9 +2495,7 @@ class Table // unlikely that this evaluates to true if (ROBIN_HOOD_UNLIKELY(mNumElements >= mMaxNumElementsAllowed)) { - if (!increase_size()) { - return std::make_pair(size_t(0), InsertionState::overflow_error); - } + increase_size(); continue; } diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp similarity index 93% rename from sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index 6e7469ff05f67..7f8c500d40f45 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -48,9 +48,9 @@ * */ -#include "pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp" -#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" #include #include @@ -58,7 +58,7 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( const rclcpp::NodeOptions & options) @@ -133,7 +133,8 @@ rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::par return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::VoxelGridDownsampleFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::pointcloud_preprocessor::VoxelGridDownsampleFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp similarity index 94% rename from sensing/pointcloud_preprocessor/src/filter.cpp rename to sensing/autoware_pointcloud_preprocessor/src/filter.cpp index 435cecec8b6f2..95e85f8450840 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -49,9 +49,9 @@ * */ -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/utility/memory.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" #include @@ -64,7 +64,7 @@ #include ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -pointcloud_preprocessor::Filter::Filter( +autoware::pointcloud_preprocessor::Filter::Filter( const std::string & filter_name, const rclcpp::NodeOptions & options) : Node(filter_name, options), filter_field_name_(filter_name) { @@ -112,20 +112,20 @@ pointcloud_preprocessor::Filter::Filter( } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void pointcloud_preprocessor::Filter::setupTF() +void autoware::pointcloud_preprocessor::Filter::setupTF() { tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void pointcloud_preprocessor::Filter::subscribe() +void autoware::pointcloud_preprocessor::Filter::subscribe() { std::string filter_name = ""; subscribe(filter_name); } -void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) +void autoware::pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) { // TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback` // each time a child class supports the faster version. @@ -165,7 +165,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void pointcloud_preprocessor::Filter::unsubscribe() +void autoware::pointcloud_preprocessor::Filter::unsubscribe() { if (use_indices_) { sub_input_filter_.unsubscribe(); @@ -183,7 +183,7 @@ void pointcloud_preprocessor::Filter::unsubscribe() ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO(sykwer): Temporary Implementation: Delete this function definition when all the filter nodes // conform to new API. -void pointcloud_preprocessor::Filter::computePublish( +void autoware::pointcloud_preprocessor::Filter::computePublish( const PointCloud2ConstPtr & input, const IndicesPtr & indices) { auto output = std::make_unique(); @@ -202,7 +202,8 @@ void pointcloud_preprocessor::Filter::computePublish( } ////////////////////////////////////////////////////////////////////////////////////////////// -rcl_interfaces::msg::SetParametersResult pointcloud_preprocessor::Filter::filterParamCallback( +rcl_interfaces::msg::SetParametersResult +autoware::pointcloud_preprocessor::Filter::filterParamCallback( const std::vector & p) { std::scoped_lock lock(mutex_); @@ -224,7 +225,7 @@ rcl_interfaces::msg::SetParametersResult pointcloud_preprocessor::Filter::filter ////////////////////////////////////////////////////////////////////////////////////////////// // TODO(sykwer): Temporary Implementation: Delete this function definition when all the filter nodes // conform to new API. -void pointcloud_preprocessor::Filter::input_indices_callback( +void autoware::pointcloud_preprocessor::Filter::input_indices_callback( const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices) { // If cloud is given, check if it's valid @@ -301,7 +302,7 @@ void pointcloud_preprocessor::Filter::input_indices_callback( // For performance reason, we get only a transformation matrix here. // The implementation is based on the one shown in the URL below. // https://github.com/ros-perception/perception_pcl/blob/628aaec1dc73ef4adea01e9d28f11eb417b948fd/pcl_ros/src/transforms.cpp#L61-L94 -bool pointcloud_preprocessor::Filter::_calculate_transform_matrix( +bool autoware::pointcloud_preprocessor::Filter::_calculate_transform_matrix( const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, const tf2_ros::Buffer & tf_buffer, Eigen::Matrix4f & eigen_transform /*output*/) { @@ -328,7 +329,7 @@ bool pointcloud_preprocessor::Filter::_calculate_transform_matrix( } // Returns false in error cases -bool pointcloud_preprocessor::Filter::calculate_transform_matrix( +bool autoware::pointcloud_preprocessor::Filter::calculate_transform_matrix( const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, TransformInfo & transform_info /*output*/) { @@ -362,7 +363,8 @@ bool pointcloud_preprocessor::Filter::calculate_transform_matrix( } // Returns false in error cases -bool pointcloud_preprocessor::Filter::convert_output_costly(std::unique_ptr & output) +bool autoware::pointcloud_preprocessor::Filter::convert_output_costly( + std::unique_ptr & output) { // In terms of performance, we should avoid using pcl_ros library function, // but this code path isn't reached in the main use case of Autoware, so it's left as is for now. @@ -410,7 +412,7 @@ bool pointcloud_preprocessor::Filter::convert_output_costly(std::unique_ptr #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using autoware_point_types::PointXYZIRCAEDT; using autoware_point_types::ReturnType; @@ -374,7 +374,7 @@ rcl_interfaces::msg::SetParametersResult DualReturnOutlierFilterComponent::param return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::DualReturnOutlierFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::DualReturnOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp similarity index 91% rename from sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp index bbf01e0bdf223..05bf3898d0cf0 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/radius_search_2d_outlier_filter_nodelet.hpp" #include #include @@ -20,7 +20,7 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { RadiusSearch2DOutlierFilterComponent::RadiusSearch2DOutlierFilterComponent( const rclcpp::NodeOptions & options) @@ -88,7 +88,8 @@ rcl_interfaces::msg::SetParametersResult RadiusSearch2DOutlierFilterComponent::p return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::RadiusSearch2DOutlierFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::pointcloud_preprocessor::RadiusSearch2DOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp similarity index 98% rename from sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 5e783f1e07418..f8024b7bd116b 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" #include "autoware_point_types/types.hpp" @@ -20,7 +20,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { using autoware_point_types::PointXYZIRADRT; @@ -419,7 +419,7 @@ float RingOutlierFilterComponent::calculateVisibilityScore( return 1.0f - num_filled_pixels; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::RingOutlierFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::RingOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp similarity index 92% rename from sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp index 8745c20a7cb0b..94dabf7ee5d9b 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp" #include #include @@ -20,7 +20,7 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { VoxelGridOutlierFilterComponent::VoxelGridOutlierFilterComponent( const rclcpp::NodeOptions & options) @@ -93,7 +93,7 @@ rcl_interfaces::msg::SetParametersResult VoxelGridOutlierFilterComponent::paramC return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::VoxelGridOutlierFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::VoxelGridOutlierFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp similarity index 92% rename from sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp index 6011c92afaeb0..53bb6ad619333 100644 --- a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_nodelet.cpp @@ -48,7 +48,7 @@ * */ -#include "pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_nodelet.hpp" #include #include @@ -56,7 +56,7 @@ #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PassThroughFilterComponent::PassThroughFilterComponent(const rclcpp::NodeOptions & options) : Filter("PassThroughFilter", options) @@ -89,7 +89,7 @@ rcl_interfaces::msg::SetParametersResult PassThroughFilterComponent::paramCallba return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PassThroughFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::PassThroughFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp similarity index 93% rename from sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp index 130e3e3de7934..cc9f3276033a1 100644 --- a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/passthrough_filter/passthrough_filter_uint16_nodelet.hpp" #include #include @@ -21,7 +21,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PassThroughFilterUInt16Component::PassThroughFilterUInt16Component( const rclcpp::NodeOptions & options) @@ -121,7 +121,7 @@ rcl_interfaces::msg::SetParametersResult PassThroughFilterUInt16Component::param return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PassThroughFilterUInt16Component) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::PassThroughFilterUInt16Component) diff --git a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp b/sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp similarity index 99% rename from sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp rename to sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp index d68ba32282351..c42abd27fe9c8 100644 --- a/sensing/pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/passthrough_filter/passthrough_uint16.cpp @@ -51,7 +51,7 @@ * */ -#include "pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp" +#include "autoware/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp" ////////////////////////////////////////////////////////////////////////// void pcl::PassThroughUInt16::applyFilter(PCLPointCloud2 & output) diff --git a/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp similarity index 90% rename from sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp index cfaffece4169f..9502add4b38ce 100644 --- a/sensing/pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/pointcloud_accumulator/pointcloud_accumulator_nodelet.hpp" #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PointcloudAccumulatorComponent::PointcloudAccumulatorComponent(const rclcpp::NodeOptions & options) : Filter("PointcloudAccumulator", options) @@ -75,7 +75,7 @@ rcl_interfaces::msg::SetParametersResult PointcloudAccumulatorComponent::paramCa return result; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointcloudAccumulatorComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::PointcloudAccumulatorComponent) diff --git a/sensing/pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp b/sensing/autoware_pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp similarity index 91% rename from sensing/pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp rename to sensing/autoware_pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp index 6cdfc8bbe2c05..71b4c2026396e 100644 --- a/sensing/pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/polygon_remover/polygon_remover.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/polygon_remover/polygon_remover.hpp" +#include "autoware/pointcloud_preprocessor/polygon_remover/polygon_remover.hpp" -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PolygonRemoverComponent::PolygonRemoverComponent(const rclcpp::NodeOptions & options) : Filter("PolygonRemover", options) @@ -70,7 +70,7 @@ void PolygonRemoverComponent::filter( void PolygonRemoverComponent::update_polygon( const geometry_msgs::msg::Polygon::ConstSharedPtr & polygon_in) { - pointcloud_preprocessor::utils::to_cgal_polygon(*polygon_in, polygon_cgal_); + autoware::pointcloud_preprocessor::utils::to_cgal_polygon(*polygon_in, polygon_cgal_); if (will_visualize_) { marker_.ns = ""; marker_.id = 0; @@ -132,11 +132,11 @@ sensor_msgs::msg::PointCloud2 PolygonRemoverComponent::remove_updated_polygon_fr } PointCloud2 cloud_out; - pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + autoware::pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( *cloud_in, polygon_cgal_, cloud_out); return cloud_out; } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PolygonRemoverComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::PolygonRemoverComponent) diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp similarity index 98% rename from sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 9aba44be1065c..edbe30f8f0ea7 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -21,7 +21,7 @@ * @author Yoshi Ri */ -#include "pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp" #include @@ -37,7 +37,7 @@ #define DEFAULT_SYNC_TOPIC_POSTFIX "_synchronized" ////////////////////////////////////////////////////////////////////////////////////////////// -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) @@ -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; }); @@ -648,7 +648,8 @@ void PointCloudDataSynchronizerComponent::checkSyncStatus( : "Some topics are not concatenated"; stat.summary(level, message); } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointCloudDataSynchronizerComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::pointcloud_preprocessor::PointCloudDataSynchronizerComponent) diff --git a/sensing/pointcloud_preprocessor/src/utility/geometry.cpp b/sensing/autoware_pointcloud_preprocessor/src/utility/geometry.cpp similarity index 96% rename from sensing/pointcloud_preprocessor/src/utility/geometry.cpp rename to sensing/autoware_pointcloud_preprocessor/src/utility/geometry.cpp index e3013d05d925f..7e9604c3dd305 100644 --- a/sensing/pointcloud_preprocessor/src/utility/geometry.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/utility/geometry.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/geometry.hpp" +#include "autoware/pointcloud_preprocessor/utility/geometry.hpp" -namespace pointcloud_preprocessor::utils +namespace autoware::pointcloud_preprocessor::utils { void to_cgal_polygon(const geometry_msgs::msg::Polygon & polygon_in, PolygonCgal & polygon_out) { @@ -155,4 +155,4 @@ bool point_within_cgal_polys( return false; } -} // namespace pointcloud_preprocessor::utils +} // namespace autoware::pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/src/utility/memory.cpp b/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp similarity index 98% rename from sensing/pointcloud_preprocessor/src/utility/memory.cpp rename to sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp index 138573a2014ff..7e265bcfb4960 100644 --- a/sensing/pointcloud_preprocessor/src/utility/memory.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/memory.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" #include -namespace pointcloud_preprocessor::utils +namespace autoware::pointcloud_preprocessor::utils { bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input) { @@ -208,4 +208,4 @@ bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::Poi return same_layout; } -} // namespace pointcloud_preprocessor::utils +} // namespace autoware::pointcloud_preprocessor::utils diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp similarity index 96% rename from sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp index c9831d2641ab8..3f7c39bdeecbf 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp" -#include "pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/filter.hpp" #include @@ -30,7 +30,7 @@ #include #include -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions & node_options) : Node("LaneletMapFilter", node_options) @@ -268,7 +268,7 @@ void Lanelet2MapFilterComponent::mapCallback( road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::Lanelet2MapFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::Lanelet2MapFilterComponent) diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp similarity index 91% rename from sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp rename to sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index def7652848642..6272fe30e47c0 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp" +#include "autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp" namespace { @@ -48,12 +48,12 @@ pcl::PointCloud removePointsWithinPolygons( for (const auto & polygon : polygons) { const auto lanelet_poly = lanelet::utils::to2D(polygon).basicPolygon(); PolygonCgal cgal_poly; - pointcloud_preprocessor::utils::to_cgal_polygon(lanelet_poly, cgal_poly); + autoware::pointcloud_preprocessor::utils::to_cgal_polygon(lanelet_poly, cgal_poly); cgal_polys.emplace_back(cgal_poly); } pcl::PointCloud filtered_cloud; - pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + autoware::pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( *cloud_in, cgal_polys, filtered_cloud, z_threshold_); return filtered_cloud; @@ -61,7 +61,7 @@ pcl::PointCloud removePointsWithinPolygons( } // anonymous namespace -namespace pointcloud_preprocessor +namespace autoware::pointcloud_preprocessor { VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( const rclcpp::NodeOptions & node_options) @@ -139,7 +139,8 @@ void VectorMapInsideAreaFilterComponent::mapCallback( polygon_lanelets_ = lanelet::utils::query::getAllPolygonsByType(lanelet_map_ptr, polygon_type_); } -} // namespace pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #include -RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::VectorMapInsideAreaFilterComponent) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::pointcloud_preprocessor::VectorMapInsideAreaFilterComponent) diff --git a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp similarity index 98% rename from sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp rename to sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index 3b500bfb3e982..f90314a001105 100644 --- a/sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -25,8 +25,8 @@ // imu (6msgs): // 10.09, 10.117, 10.144, 10.171, 10.198, 10.225 +#include "autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" #include "autoware/universe_utils/math/trigonometry.hpp" -#include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp" #include @@ -48,9 +48,9 @@ class DistortionCorrectorTest : public ::testing::Test { node_ = std::make_shared("test_node"); distortion_corrector_2d_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); distortion_corrector_3d_ = - std::make_shared(node_.get()); + std::make_shared(node_.get()); // Setup TF tf_broadcaster_ = std::make_shared(node_); @@ -246,8 +246,10 @@ class DistortionCorrectorTest : public ::testing::Test } std::shared_ptr node_; - std::shared_ptr distortion_corrector_2d_; - std::shared_ptr distortion_corrector_3d_; + std::shared_ptr + distortion_corrector_2d_; + std::shared_ptr + distortion_corrector_3d_; std::shared_ptr tf_broadcaster_; static constexpr float standard_tolerance_{1e-4}; diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_utilities.cpp similarity index 89% rename from sensing/pointcloud_preprocessor/test/test_utilities.cpp rename to sensing/autoware_pointcloud_preprocessor/test/test_utilities.cpp index e3075b4b11e79..f99901896dfb6 100644 --- a/sensing/pointcloud_preprocessor/test/test_utilities.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_utilities.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_preprocessor/utility/geometry.hpp" +#include "autoware/pointcloud_preprocessor/utility/geometry.hpp" #include @@ -58,7 +58,8 @@ TEST_F(RemovePolygonCgalFromCloudTest, PointsInsidePolygonAreRemoved) sensor_msgs::msg::PointCloud2 cloud_in; CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon - pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + autoware::pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out); pcl::PointCloud pcl_output; pcl::fromROSMsg(cloud_out, pcl_output); @@ -72,7 +73,8 @@ TEST_F(RemovePolygonCgalFromCloudTest, PointsOutsidePolygonRemain) sensor_msgs::msg::PointCloud2 cloud_in; CreatePointCloud2(cloud_in, 1.5, 1.5, 0.1); // point outside the polygon - pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + autoware::pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out); pcl::PointCloud pcl_output; pcl::fromROSMsg(cloud_out, pcl_output); @@ -90,7 +92,7 @@ TEST_F(RemovePolygonCgalFromCloudTest, PointsBelowMaxZAreRemoved) CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon, below max_z std::optional max_z = 1.0f; - pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + autoware::pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( cloud_in, polygon, cloud_out, max_z); pcl::PointCloud pcl_output; @@ -106,7 +108,7 @@ TEST_F(RemovePolygonCgalFromCloudTest, PointsAboveMaxZRemain) CreatePointCloud2(cloud_in, 0.5, 0.5, 1.5); // point inside the polygon, above max_z std::optional max_z = 1.0f; - pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + autoware::pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( cloud_in, polygon, cloud_out, max_z); pcl::PointCloud pcl_output; diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/autoware_radar_scan_to_pointcloud2/CMakeLists.txt similarity index 94% rename from sensing/radar_scan_to_pointcloud2/CMakeLists.txt rename to sensing/autoware_radar_scan_to_pointcloud2/CMakeLists.txt index 44b668b49c87a..07139739067f1 100644 --- a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt +++ b/sensing/autoware_radar_scan_to_pointcloud2/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_scan_to_pointcloud2) +project(autoware_radar_scan_to_pointcloud2) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/sensing/radar_scan_to_pointcloud2/README.md b/sensing/autoware_radar_scan_to_pointcloud2/README.md similarity index 94% rename from sensing/radar_scan_to_pointcloud2/README.md rename to sensing/autoware_radar_scan_to_pointcloud2/README.md index 100fd509ef35b..507393396b379 100644 --- a/sensing/radar_scan_to_pointcloud2/README.md +++ b/sensing/autoware_radar_scan_to_pointcloud2/README.md @@ -29,5 +29,5 @@ ### How to launch ```sh -ros2 launch radar_scan_to_pointcloud2 radar_scan_to_pointcloud2.launch.xml +ros2 launch autoware_radar_scan_to_pointcloud2 radar_scan_to_pointcloud2.launch.xml ``` diff --git a/sensing/radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml b/sensing/autoware_radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml similarity index 100% rename from sensing/radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml rename to sensing/autoware_radar_scan_to_pointcloud2/config/radar_scan_to_pointcloud2.param.yaml diff --git a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml b/sensing/autoware_radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml similarity index 66% rename from sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml rename to sensing/autoware_radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml index 45853c409fced..b798a5ca23e63 100644 --- a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml +++ b/sensing/autoware_radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/autoware_radar_scan_to_pointcloud2/package.xml similarity index 89% rename from sensing/radar_scan_to_pointcloud2/package.xml rename to sensing/autoware_radar_scan_to_pointcloud2/package.xml index c8c84ee3c8a4a..7a2a8bd9694b3 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/autoware_radar_scan_to_pointcloud2/package.xml @@ -1,8 +1,8 @@ - radar_scan_to_pointcloud2 + autoware_radar_scan_to_pointcloud2 0.1.0 - radar_scan_to_pointcloud2 + autoware_radar_scan_to_pointcloud2 Satoshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/sensing/radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json b/sensing/autoware_radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json similarity index 100% rename from sensing/radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json rename to sensing/autoware_radar_scan_to_pointcloud2/schema/radar_scan_to_pointcloud2.schema.json diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp b/sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp similarity index 100% rename from sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp rename to sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.cpp diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp b/sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp similarity index 100% rename from sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp rename to sensing/autoware_radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node.hpp diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/autoware_radar_static_pointcloud_filter/CMakeLists.txt similarity index 93% rename from sensing/radar_static_pointcloud_filter/CMakeLists.txt rename to sensing/autoware_radar_static_pointcloud_filter/CMakeLists.txt index a796a0fa9734d..5fb35d34a8501 100644 --- a/sensing/radar_static_pointcloud_filter/CMakeLists.txt +++ b/sensing/autoware_radar_static_pointcloud_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_static_pointcloud_filter) +project(autoware_radar_static_pointcloud_filter) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/sensing/radar_static_pointcloud_filter/README.md b/sensing/autoware_radar_static_pointcloud_filter/README.md similarity index 93% rename from sensing/radar_static_pointcloud_filter/README.md rename to sensing/autoware_radar_static_pointcloud_filter/README.md index 1e480ef4354dc..e84e8e998806f 100644 --- a/sensing/radar_static_pointcloud_filter/README.md +++ b/sensing/autoware_radar_static_pointcloud_filter/README.md @@ -28,7 +28,7 @@ Calculation cost is O(n). `n` is the number of radar pointcloud. ### How to launch ```sh -ros2 launch radar_static_pointcloud_filter radar_static_pointcloud_filter.launch +ros2 launch autoware_radar_static_pointcloud_filter radar_static_pointcloud_filter.launch.xml ``` ### Algorithm diff --git a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml b/sensing/autoware_radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml similarity index 100% rename from sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml rename to sensing/autoware_radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml diff --git a/sensing/radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg b/sensing/autoware_radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg similarity index 100% rename from sensing/radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg rename to sensing/autoware_radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg diff --git a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml b/sensing/autoware_radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml similarity index 68% rename from sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml rename to sensing/autoware_radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml index b9491227837c5..ec2a9d0f9af27 100644 --- a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml +++ b/sensing/autoware_radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml @@ -4,9 +4,9 @@ - + - + diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/autoware_radar_static_pointcloud_filter/package.xml similarity index 88% rename from sensing/radar_static_pointcloud_filter/package.xml rename to sensing/autoware_radar_static_pointcloud_filter/package.xml index 45c706791535c..a8424be8f78eb 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/autoware_radar_static_pointcloud_filter/package.xml @@ -1,8 +1,8 @@ - radar_static_pointcloud_filter + autoware_radar_static_pointcloud_filter 0.1.0 - radar_static_pointcloud_filter + autoware_radar_static_pointcloud_filter Satoshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json b/sensing/autoware_radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json similarity index 100% rename from sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json rename to sensing/autoware_radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp b/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp similarity index 100% rename from sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp rename to sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.cpp diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp b/sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp similarity index 100% rename from sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp rename to sensing/autoware_radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node.hpp diff --git a/sensing/radar_threshold_filter/CMakeLists.txt b/sensing/autoware_radar_threshold_filter/CMakeLists.txt similarity index 95% rename from sensing/radar_threshold_filter/CMakeLists.txt rename to sensing/autoware_radar_threshold_filter/CMakeLists.txt index 136b41cb3b37b..fa315876435f7 100644 --- a/sensing/radar_threshold_filter/CMakeLists.txt +++ b/sensing/autoware_radar_threshold_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_threshold_filter) +project(autoware_radar_threshold_filter) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/sensing/radar_threshold_filter/README.md b/sensing/autoware_radar_threshold_filter/README.md similarity index 97% rename from sensing/radar_threshold_filter/README.md rename to sensing/autoware_radar_threshold_filter/README.md index 1273ae3ebe935..f53ff7119a8eb 100644 --- a/sensing/radar_threshold_filter/README.md +++ b/sensing/autoware_radar_threshold_filter/README.md @@ -44,5 +44,5 @@ Calculation cost is O(n). `n` is the number of radar return. ### How to launch ```sh -ros2 launch radar_threshold_filter radar_threshold_filter.launch.xml +ros2 launch autoware_radar_threshold_filter radar_threshold_filter.launch.xml ``` diff --git a/sensing/radar_threshold_filter/config/radar_threshold_filter.param.yaml b/sensing/autoware_radar_threshold_filter/config/radar_threshold_filter.param.yaml similarity index 100% rename from sensing/radar_threshold_filter/config/radar_threshold_filter.param.yaml rename to sensing/autoware_radar_threshold_filter/config/radar_threshold_filter.param.yaml diff --git a/sensing/radar_threshold_filter/launch/radar_threshold_filter.launch.xml b/sensing/autoware_radar_threshold_filter/launch/radar_threshold_filter.launch.xml similarity index 55% rename from sensing/radar_threshold_filter/launch/radar_threshold_filter.launch.xml rename to sensing/autoware_radar_threshold_filter/launch/radar_threshold_filter.launch.xml index 62a42f02d814b..f0b82306be10f 100644 --- a/sensing/radar_threshold_filter/launch/radar_threshold_filter.launch.xml +++ b/sensing/autoware_radar_threshold_filter/launch/radar_threshold_filter.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/sensing/radar_threshold_filter/package.xml b/sensing/autoware_radar_threshold_filter/package.xml similarity index 89% rename from sensing/radar_threshold_filter/package.xml rename to sensing/autoware_radar_threshold_filter/package.xml index 6b81f225db971..a141a81e51c52 100644 --- a/sensing/radar_threshold_filter/package.xml +++ b/sensing/autoware_radar_threshold_filter/package.xml @@ -1,8 +1,8 @@ - radar_threshold_filter + autoware_radar_threshold_filter 0.1.0 - radar_threshold_filter + autoware_radar_threshold_filter Satoshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp b/sensing/autoware_radar_threshold_filter/src/radar_threshold_filter_node.cpp similarity index 100% rename from sensing/radar_threshold_filter/src/radar_threshold_filter_node.cpp rename to sensing/autoware_radar_threshold_filter/src/radar_threshold_filter_node.cpp diff --git a/sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp b/sensing/autoware_radar_threshold_filter/src/radar_threshold_filter_node.hpp similarity index 97% rename from sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp rename to sensing/autoware_radar_threshold_filter/src/radar_threshold_filter_node.hpp index a8a25bd972112..f503638a4d01e 100644 --- a/sensing/radar_threshold_filter/src/radar_threshold_filter_node.hpp +++ b/sensing/autoware_radar_threshold_filter/src/radar_threshold_filter_node.hpp @@ -55,7 +55,7 @@ class RadarThresholdFilterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_radar_{}; // Callback - void onData(const RadarScan::ConstSharedPtr msg); + void onData(const RadarScan::ConstSharedPtr radar_msg); // Publisher rclcpp::Publisher::SharedPtr pub_radar_{}; diff --git a/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp b/sensing/autoware_radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp similarity index 100% rename from sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp rename to sensing/autoware_radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp diff --git a/sensing/radar_threshold_filter/test/test_threshold_filter.cpp b/sensing/autoware_radar_threshold_filter/test/test_threshold_filter.cpp similarity index 100% rename from sensing/radar_threshold_filter/test/test_threshold_filter.cpp rename to sensing/autoware_radar_threshold_filter/test/test_threshold_filter.cpp diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/autoware_radar_tracks_noise_filter/CMakeLists.txt similarity index 95% rename from sensing/radar_tracks_noise_filter/CMakeLists.txt rename to sensing/autoware_radar_tracks_noise_filter/CMakeLists.txt index 436c3472c8734..3cdf69f6b3a85 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/autoware_radar_tracks_noise_filter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(radar_tracks_noise_filter) +project(autoware_radar_tracks_noise_filter) # Dependencies find_package(autoware_cmake REQUIRED) diff --git a/sensing/radar_tracks_noise_filter/README.md b/sensing/autoware_radar_tracks_noise_filter/README.md similarity index 97% rename from sensing/radar_tracks_noise_filter/README.md rename to sensing/autoware_radar_tracks_noise_filter/README.md index f1251b4f4ee74..25e8d2f96e7f7 100644 --- a/sensing/radar_tracks_noise_filter/README.md +++ b/sensing/autoware_radar_tracks_noise_filter/README.md @@ -1,4 +1,4 @@ -# radar_tracks_noise_filter +# autoware_radar_tracks_noise_filter This package contains a radar object filter module for `radar_msgs/msg/RadarTrack`. This package can filter noise objects in RadarTracks. diff --git a/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml b/sensing/autoware_radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml similarity index 100% rename from sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml rename to sensing/autoware_radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml diff --git a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml b/sensing/autoware_radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml similarity index 63% rename from sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml rename to sensing/autoware_radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml index f980bcebf1b6b..60dbbe8952249 100644 --- a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml +++ b/sensing/autoware_radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml @@ -2,9 +2,9 @@ - + - + diff --git a/sensing/radar_tracks_noise_filter/package.xml b/sensing/autoware_radar_tracks_noise_filter/package.xml similarity index 89% rename from sensing/radar_tracks_noise_filter/package.xml rename to sensing/autoware_radar_tracks_noise_filter/package.xml index 8420b7a174edf..8c69eeed6757b 100644 --- a/sensing/radar_tracks_noise_filter/package.xml +++ b/sensing/autoware_radar_tracks_noise_filter/package.xml @@ -1,9 +1,9 @@ - radar_tracks_noise_filter + autoware_radar_tracks_noise_filter 0.1.0 - radar_tracks_noise_filter + autoware_radar_tracks_noise_filter Sathshi Tanaka Shunsuke Miura Yoshi Ri diff --git a/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp b/sensing/autoware_radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp similarity index 100% rename from sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp rename to sensing/autoware_radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.cpp diff --git a/sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp b/sensing/autoware_radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp similarity index 100% rename from sensing/radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp rename to sensing/autoware_radar_tracks_noise_filter/src/radar_tracks_noise_filter_node.hpp diff --git a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/autoware_radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp similarity index 100% rename from sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp rename to sensing/autoware_radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp diff --git a/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp b/sensing/autoware_radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp similarity index 100% rename from sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp rename to sensing/autoware_radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp index dfa152d32c0d9..9eff50d296a95 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -31,7 +31,7 @@ class GyroBiasEstimationModule void update_bias( const std::vector & pose_list, const std::vector & gyro_list); - geometry_msgs::msg::Vector3 get_bias_base_link() const; + [[nodiscard]] geometry_msgs::msg::Vector3 get_bias_base_link() const; private: std::pair gyro_bias_pair_; diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index f96bab16443fe..c6e12d2481f89 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -23,7 +23,7 @@ #include -std::array transformCovariance(const std::array & cov) +std::array transform_covariance(const std::array & cov) { using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -32,7 +32,7 @@ std::array transformCovariance(const std::array & cov) max_cov = std::max(max_cov, cov[COV_IDX::Y_Y]); max_cov = std::max(max_cov, cov[COV_IDX::Z_Z]); - std::array cov_transformed; + std::array cov_transformed{}; cov_transformed.fill(0.); cov_transformed[COV_IDX::X_X] = max_cov; cov_transformed[COV_IDX::Y_Y] = max_cov; @@ -40,7 +40,7 @@ std::array transformCovariance(const std::array & cov) return cov_transformed; } -geometry_msgs::msg::Vector3 transformVector3( +geometry_msgs::msg::Vector3 transform_vector3( const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform) { geometry_msgs::msg::Vector3Stamped vec_stamped; @@ -73,12 +73,12 @@ ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) accel_stddev_imu_link_ = declare_parameter("acceleration_stddev", 10000.0); imu_sub_ = create_subscription( - "input", rclcpp::QoS{1}, std::bind(&ImuCorrector::callbackImu, this, std::placeholders::_1)); + "input", rclcpp::QoS{1}, std::bind(&ImuCorrector::callback_imu, this, std::placeholders::_1)); imu_pub_ = create_publisher("output", rclcpp::QoS{10}); } -void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void ImuCorrector::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { sensor_msgs::msg::Imu imu_msg; imu_msg = *imu_msg_ptr; @@ -113,12 +113,13 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m imu_msg_base_link.header.stamp = imu_msg_ptr->header.stamp; imu_msg_base_link.header.frame_id = output_frame_; imu_msg_base_link.linear_acceleration = - transformVector3(imu_msg.linear_acceleration, *tf_imu2base_ptr); + transform_vector3(imu_msg.linear_acceleration, *tf_imu2base_ptr); imu_msg_base_link.linear_acceleration_covariance = - transformCovariance(imu_msg.linear_acceleration_covariance); - imu_msg_base_link.angular_velocity = transformVector3(imu_msg.angular_velocity, *tf_imu2base_ptr); + transform_covariance(imu_msg.linear_acceleration_covariance); + imu_msg_base_link.angular_velocity = + transform_vector3(imu_msg.angular_velocity, *tf_imu2base_ptr); imu_msg_base_link.angular_velocity_covariance = - transformCovariance(imu_msg.angular_velocity_covariance); + transform_covariance(imu_msg.angular_velocity_covariance); imu_pub_->publish(imu_msg_base_link); } diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp index 0dacab4651477..c02aa88a313a7 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -37,7 +37,7 @@ class ImuCorrector : public rclcpp::Node explicit ImuCorrector(const rclcpp::NodeOptions & options); private: - void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); rclcpp::Subscription::SharedPtr imu_sub_; diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp index 78558feeb7936..257552e15cbab 100644 --- a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -63,7 +63,9 @@ TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) { - ASSERT_THROW(module.get_bias_base_link(), std::runtime_error); + // for the case of method with [[nodiscard]] attribute + geometry_msgs::msg::Vector3 bias_base_link; + ASSERT_THROW(bias_base_link = module.get_bias_base_link(), std::runtime_error); } TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 8ef46609ee542..5f7b1c044d7e3 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -29,10 +29,9 @@ class VehicleVelocityConverter : public rclcpp::Node { public: explicit VehicleVelocityConverter(const rclcpp::NodeOptions & options); - ~VehicleVelocityConverter() = default; private: - void callbackVelocityReport(const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg); + void callback_velocity_report(const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg); rclcpp::Subscription::SharedPtr vehicle_report_sub_; @@ -43,7 +42,6 @@ class VehicleVelocityConverter : public rclcpp::Node double stddev_vx_; double stddev_wz_; double speed_scale_factor_; - std::array twist_covariance_; }; #endif // VEHICLE_VELOCITY_CONVERTER__VEHICLE_VELOCITY_CONVERTER_HPP_ diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 2aece69f5283b..c382f0f16f6e5 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -25,13 +25,13 @@ VehicleVelocityConverter::VehicleVelocityConverter(const rclcpp::NodeOptions & o vehicle_report_sub_ = create_subscription( "velocity_status", rclcpp::QoS{100}, - std::bind(&VehicleVelocityConverter::callbackVelocityReport, this, std::placeholders::_1)); + std::bind(&VehicleVelocityConverter::callback_velocity_report, this, std::placeholders::_1)); twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); } -void VehicleVelocityConverter::callbackVelocityReport( +void VehicleVelocityConverter::callback_velocity_report( const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { if (msg->header.frame_id != frame_id_) { 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/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 3257f2bb1200c..41025f74cbe50 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -24,7 +24,7 @@ - + @@ -45,7 +45,7 @@ - + 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..d550e335e2076 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -0,0 +1,260 @@ +// 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 acc = std::clamp(state(IDX::ACCX), -vx_rate_lim_, vx_rate_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_wo_slope = 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_des = 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_wo_slope + acc_by_slope; + } + return acc_des_wo_slope + 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/diagnostic_graph_aggregator/src/common/graph/config.cpp b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp index 496c95722002a..b91b777831368 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp @@ -253,7 +253,7 @@ void apply_edits(FileConfig & config) config.links = std::move(filtered_links); } -void topological_sort(FileConfig & config) +void topological_sort(const FileConfig & config) { std::unordered_map degrees; std::deque units; @@ -264,9 +264,9 @@ void topological_sort(FileConfig & config) for (const auto & unit : config.units) units.push_back(unit.get()); // Count degrees of each unit. - for (const auto & unit : units) { - if (const auto & link = unit->item) ++degrees[link->child]; - for (const auto & link : unit->list) ++degrees[link->child]; + for (const auto * const unit : units) { + if (const auto * const link = unit->item) ++degrees[link->child]; + for (const auto * const link : unit->list) ++degrees[link->child]; } // Find initial units that are zero degrees. diff --git a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp b/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp index 74636adab7415..e4d2e470a0d32 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp @@ -59,7 +59,7 @@ UnitLink * UnitLoader::child() const std::vector UnitLoader::children() const { std::vector result; - for (const auto & config : config_->list) { + for (auto * const config : config_->list) { result.push_back(links_.config_links.at(config)); } return result; diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp index d9cbdbaa64400..5ebd603964add 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/units.cpp @@ -145,7 +145,7 @@ MaxUnit::MaxUnit(const UnitLoader & unit) : NodeUnit(unit) void MaxUnit::update_status() { DiagnosticLevel level = DiagnosticStatus::OK; - for (const auto & link : links_) { + for (const auto * const link : links_) { level = std::max(level, link->child()->level()); } status_.level = std::min(level, DiagnosticStatus::ERROR); @@ -155,7 +155,7 @@ void ShortCircuitMaxUnit::update_status() { // TODO(Takagi, Isamu): update link flags. DiagnosticLevel level = DiagnosticStatus::OK; - for (const auto & link : links_) { + for (const auto * const link : links_) { level = std::max(level, link->child()->level()); } status_.level = std::min(level, DiagnosticStatus::ERROR); @@ -171,7 +171,7 @@ void MinUnit::update_status() DiagnosticLevel level = DiagnosticStatus::OK; if (!links_.empty()) { level = DiagnosticStatus::STALE; - for (const auto & link : links_) { + for (const auto * const link : links_) { level = std::min(level, link->child()->level()); } } 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/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/system_diagnostic_monitor/config/autoware-psim.yaml b/system/system_diagnostic_monitor/config/autoware-psim.yaml index 3d55079229280..499356a3334cc 100644 --- a/system/system_diagnostic_monitor/config/autoware-psim.yaml +++ b/system/system_diagnostic_monitor/config/autoware-psim.yaml @@ -8,4 +8,3 @@ edits: - { type: remove, path: /autoware/localization/sensor_fusion_status } - { type: remove, path: /autoware/localization/topic_rate_check/pose_twist_fusion } - { type: remove, path: /autoware/perception/topic_rate_check/pointcloud } - - { type: remove, path: /autoware/control/emergency_braking } diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index 02d7a3872953a..a5d1be613ee5a 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -272,6 +272,7 @@ void SubscriberBase::on_trajectory( if (zero_vel_idx) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp; @@ -363,6 +364,7 @@ void SubscriberBase::on_pointcloud( if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp; @@ -440,6 +442,7 @@ void SubscriberBase::on_predicted_objects( if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp; @@ -520,6 +523,7 @@ void SubscriberBase::on_detected_objects( if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); // set header time + // cppcheck-suppress constVariableReference auto & buffer = std::get(variant); buffer->header.stamp = msg_ptr->header.stamp; buffer->published_stamp = msg_ptr->header.stamp;