diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 6b4daaa4b1773..e35b948e6153e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -4,10 +4,12 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_auto_geometry/** satoshi.ota@tier4.jp -common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp +common/autoware_auto_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_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/** khalil@leodrive.ai +common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/** team-spatzenhirn@uni-ulm.de common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp common/component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/component_interface_tools/** isamu.takagi@tier4.jp @@ -22,10 +24,10 @@ common/grid_map_utils/** maxime.clement@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/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/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp +common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp -common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp +common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -33,7 +35,7 @@ common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp +common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp @@ -58,7 +60,7 @@ common/traffic_light_utils/** mingyu.li@tier4.jp satoshi.ota@tier4.jp shunsuke.m common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp +control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp @@ -76,10 +78,11 @@ evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4 evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp +evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -launch/tier4_map_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @@ -106,7 +109,7 @@ localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tie localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp +map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_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/** ryohsuke.mitsudome@tier4.jp @@ -122,11 +125,11 @@ perception/euclidean_cluster/** yukihiro.saito@tier4.jp perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/heatmap_visualizer/** kotaro.uetake@tier4.jp -perception/image_projection_based_fusion/** dai.nguyen@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/image_projection_based_fusion/** dai.nguyen@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp +perception/lidar_centerpoint/** kenzo.lobos@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -135,11 +138,11 @@ perception/object_range_splitter/** yukihiro.saito@tier4.jp perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp +perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/radar_object_tracker/** satoshi.tanaka@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@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/** mingyu.li@tier4.jp @@ -158,12 +161,12 @@ planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4. planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/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 -planning/behavior_path_goal_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_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 planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_path_start_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -187,7 +190,7 @@ planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hir planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp +planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp @@ -199,7 +202,7 @@ planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4. planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp -planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp +planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp @@ -214,10 +217,10 @@ sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp -sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@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/** ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e95f4d46a147b..e9db0d140d947 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -36,6 +36,9 @@ jobs: with: fetch-depth: 0 + - name: Check disk space before build + run: df -h + - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -69,6 +72,9 @@ jobs: verbose: true flags: differential + - name: Check disk space after build + run: df -h + clang-tidy-differential: runs-on: [self-hosted, linux, X64] container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml deleted file mode 100644 index 1fbf2ff46925c..0000000000000 --- a/.github/workflows/spell-check-differential.yaml +++ /dev/null @@ -1,16 +0,0 @@ -name: spell-check-differential - -on: - pull_request: - -jobs: - spell-check-differential: - runs-on: ubuntu-latest - steps: - - name: Check out repository - uses: actions/checkout@v4 - - - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@v1 - with: - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c1ad3e3df140e..2033239824d95 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -8,7 +8,8 @@ Satoshi Tanaka Taiki Tanaka Takeshi Miura - + Shunsuke Miura + Yoshi Ri Apache 2.0 ament_cmake diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt new file mode 100644 index 0000000000000..da67a6f63aeae --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt @@ -0,0 +1,140 @@ +cmake_minimum_required(VERSION 3.8) +project(awf_2d_overlay_vehicle) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(autoware_auto_vehicle_msgs REQUIRED) +find_package(tier4_planning_msgs REQUIRED) +find_package(autoware_perception_msgs REQUIRED) +ament_auto_find_build_dependencies() + +find_package(rviz_2d_overlay_msgs REQUIRED) + +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(std_msgs REQUIRED) + +set( + headers_to_moc + include/overlay_text_display.hpp + include/signal_display.hpp + +) + +set( + headers_to_not_moc + include/steering_wheel_display.hpp + include/gear_display.hpp + include/speed_display.hpp + include/turn_signals_display.hpp + include/traffic_display.hpp + include/speed_limit_display.hpp +) + + + +foreach(header "${headers_to_moc}") + qt5_wrap_cpp(display_moc_files "${header}") +endforeach() + +set( + display_source_files + src/overlay_text_display.cpp + src/overlay_utils.cpp + src/signal_display.cpp + src/steering_wheel_display.cpp + src/gear_display.cpp + src/speed_display.cpp + src/turn_signals_display.cpp + src/traffic_display.cpp + src/speed_limit_display.cpp + +) + +add_library( + ${PROJECT_NAME} SHARED + ${display_moc_files} + ${headers_to_not_moc} + ${display_source_files} +) + +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) +target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic) + +target_include_directories( + ${PROJECT_NAME} PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries( + ${PROJECT_NAME} PUBLIC + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay +) + + +target_compile_definitions(${PROJECT_NAME} PRIVATE "${PROJECT_NAME}_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_target_dependencies( + ${PROJECT_NAME} + PUBLIC + rviz_common + rviz_rendering + rviz_2d_overlay_msgs + autoware_auto_vehicle_msgs + tier4_planning_msgs + autoware_perception_msgs +) + +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rviz_common + rviz_ogre_vendor +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + DESTINATION lib/${PROJECT_NAME} +) + +# Copy the assets directory to the installation directory +install( + DIRECTORY assets/ + DESTINATION share/${PROJECT_NAME}/assets +) + +add_definitions(-DQT_NO_KEYWORDS) + +ament_package( + CONFIG_EXTRAS "awf_2d_overlay_vehicle-extras.cmake" +) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE new file mode 100644 index 0000000000000..a2fe2d3d1c7ed --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE @@ -0,0 +1,12 @@ +Copyright (c) 2022, Team Spatzenhirn +Copyright (c) 2014, JSK Lab + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md new file mode 100644 index 0000000000000..6728f26878e10 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md @@ -0,0 +1,54 @@ +# awf_2d_overlay_vehicle + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +Based on the [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) +package, under the 3-Clause BSD license. + +## Purpose + +This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal, steering status and gears. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | + +## Parameter + +### Core Parameters + +#### SignalDisplay + +| Name | Type | Default Value | Description | +| ------------------------ | ------ | -------------------- | --------------------------------- | +| `property_width_` | int | 128 | Width of the plotter window [px] | +| `property_height_` | int | 128 | Height of the plotter window [px] | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_signal_color_` | QColor | QColor(25, 255, 240) | Turn Signal color | + +## Assumptions / Known limits + +TBD. + +## Usage + +1. Start rviz and select Add under the Displays panel. + + ![select_add](./assets/images/select_add.png) + +2. Select any one of the tier4_vehicle_rviz_plugin and press OK. + + ![select_vehicle_plugin](./assets/images/select_vehicle_plugin.png) + +3. Enter the name of the topic where you want to view the status. + + ![select_topic_name](./assets/images/select_topic_name.png) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE new file mode 100644 index 0000000000000..cc04d393573f0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE @@ -0,0 +1,93 @@ +Copyright 2011 The Quicksand Project Authors (https://github.com/andrew-paglinawan/QuicksandFamily), with Reserved Font Name “Quicksand”. + +This Font Software is licensed under the SIL Open Font License, Version 1.1. +This license is copied below, and is also available with a FAQ at: +http://scripts.sil.org/OFL + + +----------------------------------------------------------- +SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007 +----------------------------------------------------------- + +PREAMBLE +The goals of the Open Font License (OFL) are to stimulate worldwide +development of collaborative font projects, to support the font creation +efforts of academic and linguistic communities, and to provide a free and +open framework in which fonts may be shared and improved in partnership +with others. + +The OFL allows the licensed fonts to be used, studied, modified and +redistributed freely as long as they are not sold by themselves. The +fonts, including any derivative works, can be bundled, embedded, +redistributed and/or sold with any software provided that any reserved +names are not used by derivative works. The fonts and derivatives, +however, cannot be released under any other type of license. The +requirement for fonts to remain under this license does not apply +to any document created using the fonts or their derivatives. + +DEFINITIONS +"Font Software" refers to the set of files released by the Copyright +Holder(s) under this license and clearly marked as such. This may +include source files, build scripts and documentation. + +"Reserved Font Name" refers to any names specified as such after the +copyright statement(s). + +"Original Version" refers to the collection of Font Software components as +distributed by the Copyright Holder(s). + +"Modified Version" refers to any derivative made by adding to, deleting, +or substituting -- in part or in whole -- any of the components of the +Original Version, by changing formats or by porting the Font Software to a +new environment. + +"Author" refers to any designer, engineer, programmer, technical +writer or other person who contributed to the Font Software. + +PERMISSION & CONDITIONS +Permission is hereby granted, free of charge, to any person obtaining +a copy of the Font Software, to use, study, copy, merge, embed, modify, +redistribute, and sell modified and unmodified copies of the Font +Software, subject to the following conditions: + +1) Neither the Font Software nor any of its individual components, +in Original or Modified Versions, may be sold by itself. + +2) Original or Modified Versions of the Font Software may be bundled, +redistributed and/or sold with any software, provided that each copy +contains the above copyright notice and this license. These can be +included either as stand-alone text files, human-readable headers or +in the appropriate machine-readable metadata fields within text or +binary files as long as those fields can be easily viewed by the user. + +3) No Modified Version of the Font Software may use the Reserved Font +Name(s) unless explicit written permission is granted by the corresponding +Copyright Holder. This restriction only applies to the primary font name as +presented to the users. + +4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font +Software shall not be used to promote, endorse or advertise any +Modified Version, except to acknowledge the contribution(s) of the +Copyright Holder(s) and the Author(s) or with their explicit written +permission. + +5) The Font Software, modified or unmodified, in part or in whole, +must be distributed entirely under this license, and must not be +distributed under any other license. The requirement for fonts to +remain under this license does not apply to any document created +using the Font Software. + +TERMINATION +This license becomes null and void if any of the above conditions are +not met. + +DISCLAIMER +THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT +OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE +COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM +OTHER DEALINGS IN THE FONT SOFTWARE. diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf new file mode 100644 index 0000000000000..07d5127c04b17 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf new file mode 100644 index 0000000000000..800531084fa6c Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf new file mode 100644 index 0000000000000..f4634cd7c3f16 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf new file mode 100644 index 0000000000000..60323ed6abdf1 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf new file mode 100644 index 0000000000000..52059c3a3da3f Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png new file mode 100644 index 0000000000000..18de535ce4489 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png new file mode 100644 index 0000000000000..c5130b6092384 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png new file mode 100644 index 0000000000000..5466bcf0050df Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png new file mode 100644 index 0000000000000..863f63d728616 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png new file mode 100644 index 0000000000000..960985a3da5de Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png new file mode 100644 index 0000000000000..07b1e33132e43 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake new file mode 100644 index 0000000000000..9d6f156555e57 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake @@ -0,0 +1,31 @@ +# Copyright (c) 2021, Open Source Robotics Foundation, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# find package Qt5 because otherwise using the rviz_default_plugins::rviz_default_plugins +# exported target will complain that the Qt5::Widgets target does not exist +find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp new file mode 100644 index 0000000000000..b289bfc541cee --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEAR_DISPLAY_HPP_ +#define GEAR_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class GearDisplay +{ +public: + GearDisplay(); + void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + +private: + int current_gear_; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // GEAR_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp new file mode 100644 index 0000000000000..9b3ecccf6301c --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp @@ -0,0 +1,157 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#ifndef OVERLAY_TEXT_DISPLAY_HPP_ +#define OVERLAY_TEXT_DISPLAY_HPP_ + +#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" +#ifndef Q_MOC_RUN +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#endif + +namespace awf_2d_overlay_vehicle +{ +class OverlayTextDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT +public: + OverlayTextDisplay(); + virtual ~OverlayTextDisplay(); + +protected: + awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + + int texture_width_; + int texture_height_; + + bool overtake_fg_color_properties_; + bool overtake_bg_color_properties_; + bool overtake_position_properties_; + bool align_bottom_; + bool invert_shadow_; + QColor bg_color_; + QColor fg_color_; + int text_size_; + int line_width_; + std::string text_; + QStringList font_families_; + std::string font_; + int horizontal_dist_; + int vertical_dist_; + HorizontalAlignment horizontal_alignment_; + VerticalAlignment vertical_alignment_; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + bool require_update_texture_; + // properties are raw pointers since they are owned by Qt + rviz_common::properties::BoolProperty * overtake_position_properties_property_; + rviz_common::properties::BoolProperty * overtake_fg_color_properties_property_; + rviz_common::properties::BoolProperty * overtake_bg_color_properties_property_; + rviz_common::properties::BoolProperty * align_bottom_property_; + rviz_common::properties::BoolProperty * invert_shadow_property_; + rviz_common::properties::IntProperty * hor_dist_property_; + rviz_common::properties::IntProperty * ver_dist_property_; + rviz_common::properties::EnumProperty * hor_alignment_property_; + rviz_common::properties::EnumProperty * ver_alignment_property_; + rviz_common::properties::IntProperty * width_property_; + rviz_common::properties::IntProperty * height_property_; + rviz_common::properties::IntProperty * text_size_property_; + rviz_common::properties::IntProperty * line_width_property_; + rviz_common::properties::ColorProperty * bg_color_property_; + rviz_common::properties::FloatProperty * bg_alpha_property_; + rviz_common::properties::ColorProperty * fg_color_property_; + rviz_common::properties::FloatProperty * fg_alpha_property_; + rviz_common::properties::EnumProperty * font_property_; + +protected Q_SLOTS: + void updateOvertakePositionProperties(); + void updateOvertakeFGColorProperties(); + void updateOvertakeBGColorProperties(); + void updateAlignBottom(); + void updateInvertShadow(); + void updateHorizontalDistance(); + void updateVerticalDistance(); + void updateHorizontalAlignment(); + void updateVerticalAlignment(); + void updateWidth(); + void updateHeight(); + void updateTextSize(); + void updateFGColor(); + void updateFGAlpha(); + void updateBGColor(); + void updateBGAlpha(); + void updateFont(); + void updateLineWidth(); + +private: + void processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) override; +}; +} // namespace awf_2d_overlay_vehicle + +#endif // OVERLAY_TEXT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp new file mode 100644 index 0000000000000..1270f15ca80ae --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp @@ -0,0 +1,141 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef OVERLAY_UTILS_HPP_ +#define OVERLAY_UTILS_HPP_ + +#include +#include + +#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace awf_2d_overlay_vehicle +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; +}; + +enum class VerticalAlignment : uint8_t { + CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER, + TOP = rviz_2d_overlay_msgs::msg::OverlayText::TOP, + BOTTOM = rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM, +}; + +enum class HorizontalAlignment : uint8_t { + LEFT = rviz_2d_overlay_msgs::msg::OverlayText::LEFT, + RIGHT = rviz_2d_overlay_msgs::msg::OverlayText::RIGHT, + CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER +}; + +/** + * Helper class for realizing an overlay object on top of the rviz 3D panel. + * + * This class is supposed to be instantiated in the onInitialize method of the + * rviz_common::Display class. + */ +class OverlayObject +{ +public: + using SharedPtr = std::shared_ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName() const; + virtual void hide(); + virtual void show(); + virtual bool isTextureReady() const; + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment = HorizontalAlignment::LEFT, + VerticalAlignment ver_alignment = VerticalAlignment::TOP); + virtual void setDimensions(double width, double height); + virtual bool isVisible() const; + virtual unsigned int getTextureWidth() const; + virtual unsigned int getTextureHeight() const; + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; +}; +} // namespace awf_2d_overlay_vehicle + +#endif // OVERLAY_UTILS_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp new file mode 100644 index 0000000000000..aff475aba2f33 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp @@ -0,0 +1,124 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIGNAL_DISPLAY_HPP_ +#define SIGNAL_DISPLAY_HPP_ +#ifndef Q_MOC_RUN +#include "gear_display.hpp" +#include "overlay_utils.hpp" +#include "speed_display.hpp" +#include "speed_limit_display.hpp" +#include "steering_wheel_display.hpp" +#include "traffic_display.hpp" +#include "turn_signals_display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#endif + +namespace awf_2d_overlay_vehicle +{ +class SignalDisplay : public rviz_common::Display +{ + Q_OBJECT +public: + SignalDisplay(); + ~SignalDisplay() override; + +protected: + void onInitialize() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateOverlaySize(); + void updateSmallOverlaySize(); + void updateOverlayPosition(); + void updateOverlayColor(); + void topic_updated_gear(); + void topic_updated_steering(); + void topic_updated_speed(); + void topic_updated_speed_limit(); + void topic_updated_turn_signals(); + void topic_updated_hazard_lights(); + void topic_updated_traffic(); + +private: + std::mutex mutex_; + awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + rviz_common::properties::IntProperty * property_width_; + rviz_common::properties::IntProperty * property_height_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::ColorProperty * property_signal_color_; + std::unique_ptr steering_topic_property_; + std::unique_ptr gear_topic_property_; + std::unique_ptr speed_topic_property_; + std::unique_ptr turn_signals_topic_property_; + std::unique_ptr hazard_lights_topic_property_; + std::unique_ptr traffic_topic_property_; + std::unique_ptr speed_limit_topic_property_; + + void drawBackground(QPainter & painter, const QRectF & backgroundRect); + void setupRosSubscriptions(); + + std::unique_ptr steering_wheel_display_; + std::unique_ptr gear_display_; + std::unique_ptr speed_display_; + std::unique_ptr turn_signals_display_; + std::unique_ptr traffic_display_; + std::unique_ptr speed_limit_display_; + + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr + turn_signals_sub_; + rclcpp::Subscription::SharedPtr + hazard_lights_sub_; + rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr speed_limit_sub_; + + std::mutex property_mutex_; + + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void drawWidget(QImage & hud); +}; +} // namespace awf_2d_overlay_vehicle + +#endif // SIGNAL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp new file mode 100644 index 0000000000000..317e45917f927 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPEED_DISPLAY_HPP_ +#define SPEED_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SpeedDisplay +{ +public: + SpeedDisplay(); + void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + +private: + float current_speed_; // Internal variable to store current speed + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // SPEED_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp new file mode 100644 index 0000000000000..00eae077ff2ac --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPEED_LIMIT_DISPLAY_HPP_ +#define SPEED_LIMIT_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SpeedLimitDisplay +{ +public: + SpeedLimitDisplay(); + void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + +private: + float current_limit; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // SPEED_LIMIT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp new file mode 100644 index 0000000000000..a8291064c3807 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_WHEEL_DISPLAY_HPP_ +#define STEERING_WHEEL_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SteeringWheelDisplay +{ +public: + SteeringWheelDisplay(); + void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + +private: + float steering_angle_ = 0.0f; + QColor gray = QColor(194, 194, 194); + + QImage wheelImage; + QImage scaledWheelImage; + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // STEERING_WHEEL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp new file mode 100644 index 0000000000000..bf776d27dfa94 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_DISPLAY_HPP_ +#define TRAFFIC_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class TrafficDisplay +{ +public: + TrafficDisplay(); + void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficSignalArray current_traffic_; + +private: + QImage traffic_light_image_; + // yellow #CFC353 + QColor yellow = QColor(207, 195, 83); + // red #CF5353 + QColor red = QColor(207, 83, 83); + // green #53CF5F + QColor green = QColor(83, 207, 95); + // gray #C2C2C2 + QColor gray = QColor(194, 194, 194); + + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // TRAFFIC_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp new file mode 100644 index 0000000000000..cd659883c9ca0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TURN_SIGNALS_DISPLAY_HPP_ +#define TURN_SIGNALS_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace awf_2d_overlay_vehicle +{ + +class TurnSignalsDisplay +{ +public: + TurnSignalsDisplay(); + void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + +private: + QImage arrowImage; + QColor gray = QColor(194, 194, 194); + + int current_turn_signal_; // Internal variable to store turn signal state + int current_hazard_lights_; // Internal variable to store hazard lights state + QImage coloredImage(const QImage & source, const QColor & color); + + std::chrono::steady_clock::time_point last_toggle_time_; + bool blink_on_ = false; + const std::chrono::milliseconds blink_interval_{500}; // Blink interval in milliseconds +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // TURN_SIGNALS_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml new file mode 100644 index 0000000000000..b19b384d020b6 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml @@ -0,0 +1,31 @@ + + + + awf_2d_overlay_vehicle + 0.0.1 + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + + Khalil Selyan + + BSD-3-Clause + + autoware_auto_vehicle_msgs + autoware_perception_msgs + boost + rviz_2d_overlay_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml new file mode 100644 index 0000000000000..8a5af0abcf0dd --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml @@ -0,0 +1,5 @@ + + + Signal overlay plugin for the 3D view. + + diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp new file mode 100644 index 0000000000000..153e106f04264 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp @@ -0,0 +1,98 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gear_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +GearDisplay::GearDisplay() : current_gear_(0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void GearDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + current_gear_ = msg->report; // Assuming msg->report contains the gear information +} + +void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // we deal with the different gears here + std::string gearString; + switch (current_gear_) { + case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + gearString = "N"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::LOW: + case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + gearString = "L"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::NONE: + case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + gearString = "P"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + gearString = "R"; + break; + // all the drive gears from DRIVE to DRIVE_16 + default: + gearString = "D"; + break; + } + + QFont gearFont("Quicksand", 16, QFont::Bold); + painter.setFont(gearFont); + QPen borderPen(gray); + borderPen.setWidth(4); + painter.setPen(borderPen); + + int gearBoxSize = 30; + int gearX = backgroundRect.left() + 30 + gearBoxSize; + int gearY = backgroundRect.height() - gearBoxSize - 20; + QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize); + painter.setBrush(QColor(0, 0, 0, 0)); + painter.drawRoundedRect(gearRect, 5, 5); + painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString)); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp new file mode 100644 index 0000000000000..053d0f6e981c0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp @@ -0,0 +1,556 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_text_display.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace awf_2d_overlay_vehicle +{ +OverlayTextDisplay::OverlayTextDisplay() +: texture_width_(0), + texture_height_(0), + bg_color_(0, 0, 0, 0), + fg_color_(255, 255, 255, 255.0), + text_size_(14), + line_width_(2), + text_(""), + font_(""), + require_update_texture_(false) +{ + overtake_position_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake Position Properties", false, + "overtake position properties specified by message such as left, top and font", this, + SLOT(updateOvertakePositionProperties())); + overtake_fg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake FG Color Properties", false, + "overtake color properties specified by message such as foreground color and alpha", this, + SLOT(updateOvertakeFGColorProperties())); + overtake_bg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake BG Color Properties", false, + "overtake color properties specified by message such as background color and alpha", this, + SLOT(updateOvertakeBGColorProperties())); + align_bottom_property_ = new rviz_common::properties::BoolProperty( + "Align Bottom", false, "align text with the bottom of the overlay region", this, + SLOT(updateAlignBottom())); + invert_shadow_property_ = new rviz_common::properties::BoolProperty( + "Invert Shadow", false, "make shadow lighter than original text", this, + SLOT(updateInvertShadow())); + hor_dist_property_ = new rviz_common::properties::IntProperty( + "hor_dist", 0, "horizontal distance to anchor", this, SLOT(updateHorizontalDistance())); + ver_dist_property_ = new rviz_common::properties::IntProperty( + "ver_dist", 0, "vertical distance to anchor", this, SLOT(updateVerticalDistance())); + hor_alignment_property_ = new rviz_common::properties::EnumProperty( + "hor_alignment", "left", "horizontal alignment of the overlay", this, + SLOT(updateHorizontalAlignment())); + hor_alignment_property_->addOption("left", rviz_2d_overlay_msgs::msg::OverlayText::LEFT); + hor_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); + hor_alignment_property_->addOption("right", rviz_2d_overlay_msgs::msg::OverlayText::RIGHT); + ver_alignment_property_ = new rviz_common::properties::EnumProperty( + "ver_alignment", "top", "vertical alignment of the overlay", this, + SLOT(updateVerticalAlignment())); + ver_alignment_property_->addOption("top", rviz_2d_overlay_msgs::msg::OverlayText::TOP); + ver_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); + ver_alignment_property_->addOption("bottom", rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM); + width_property_ = new rviz_common::properties::IntProperty( + "width", 128, "width position", this, SLOT(updateWidth())); + width_property_->setMin(0); + height_property_ = new rviz_common::properties::IntProperty( + "height", 128, "height position", this, SLOT(updateHeight())); + height_property_->setMin(0); + text_size_property_ = new rviz_common::properties::IntProperty( + "text size", 12, "text size", this, SLOT(updateTextSize())); + text_size_property_->setMin(0); + line_width_property_ = new rviz_common::properties::IntProperty( + "line width", 2, "line width", this, SLOT(updateLineWidth())); + line_width_property_->setMin(0); + fg_color_property_ = new rviz_common::properties::ColorProperty( + "Foreground Color", QColor(25, 255, 240), "Foreground Color", this, SLOT(updateFGColor())); + fg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Foreground Alpha", 0.8, "Foreground Alpha", this, SLOT(updateFGAlpha())); + fg_alpha_property_->setMin(0.0); + fg_alpha_property_->setMax(1.0); + bg_color_property_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Background Color", this, SLOT(updateBGColor())); + bg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.8, "Background Alpha", this, SLOT(updateBGAlpha())); + bg_alpha_property_->setMin(0.0); + bg_alpha_property_->setMax(1.0); + + QFontDatabase database; + font_families_ = database.families(); + font_property_ = new rviz_common::properties::EnumProperty( + "font", "DejaVu Sans Mono", "font", this, SLOT(updateFont())); + for (ssize_t i = 0; i < font_families_.size(); i++) { + font_property_->addOption(font_families_[i], static_cast(i)); + } +} + +OverlayTextDisplay::~OverlayTextDisplay() +{ + onDisable(); +} + +void OverlayTextDisplay::onEnable() +{ + if (overlay_) { + overlay_->show(); + } + subscribe(); +} + +void OverlayTextDisplay::onDisable() +{ + if (overlay_) { + overlay_->hide(); + } + unsubscribe(); +} + +// only the first time +void OverlayTextDisplay::onInitialize() +{ + RTDClass::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + + onEnable(); + updateTopic(); + updateOvertakePositionProperties(); + updateOvertakeFGColorProperties(); + updateOvertakeBGColorProperties(); + updateAlignBottom(); + updateInvertShadow(); + updateHorizontalDistance(); + updateVerticalDistance(); + updateHorizontalAlignment(); + updateVerticalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + updateFGColor(); + updateFGAlpha(); + updateBGColor(); + updateBGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; +} + +void OverlayTextDisplay::update(float /*wall_dt*/, float /*ros_dt*/) +{ + if (!require_update_texture_) { + return; + } + if (!isEnabled()) { + return; + } + if (!overlay_) { + return; + } + + overlay_->updateTextureSize(texture_width_, texture_height_); + { + awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage Hud = buffer.getQImage(*overlay_, bg_color_); + QPainter painter(&Hud); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setPen(QPen(fg_color_, std::max(line_width_, 1), Qt::SolidLine)); + uint16_t w = overlay_->getTextureWidth(); + uint16_t h = overlay_->getTextureHeight(); + + // font + if (text_size_ != 0) { + // QFont font = painter.font(); + QFont font(font_.length() > 0 ? font_.c_str() : "Liberation Sans"); + font.setPointSize(text_size_); + font.setBold(true); + painter.setFont(font); + } + if (text_.length() > 0) { + QColor shadow_color; + if (invert_shadow_) + shadow_color = Qt::white; // fg_color_.lighter(); + else + shadow_color = Qt::black; // fg_color_.darker(); + shadow_color.setAlpha(fg_color_.alpha()); + + std::string color_wrapped_text = + (boost::format("%1%") % text_ % + fg_color_.red() % fg_color_.green() % fg_color_.blue() % fg_color_.alpha()) + .str(); + + // find a remove "color: XXX;" regex match to generate a proper shadow + std::regex color_tag_re("color:.+?;"); + std::string null_char(""); + std::string formatted_text_ = std::regex_replace(text_, color_tag_re, null_char); + std::string color_wrapped_shadow = + (boost::format("%1%") % + formatted_text_ % shadow_color.red() % shadow_color.green() % shadow_color.blue() % + shadow_color.alpha()) + .str(); + + QStaticText static_text( + boost::algorithm::replace_all_copy(color_wrapped_text, "\n", "
").c_str()); + static_text.setTextWidth(w); + + painter.setPen(QPen(shadow_color, std::max(line_width_, 1), Qt::SolidLine)); + QStaticText static_shadow( + boost::algorithm::replace_all_copy(color_wrapped_shadow, "\n", "
").c_str()); + static_shadow.setTextWidth(w); + + if (!align_bottom_) { + painter.drawStaticText(1, 1, static_shadow); + painter.drawStaticText(0, 0, static_text); + } else { + QStaticText only_wrapped_text(color_wrapped_text.c_str()); + QFontMetrics fm(painter.fontMetrics()); + QRect text_rect = fm.boundingRect( + 0, 0, w, h, Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop, + only_wrapped_text.text().remove(QRegExp("<[^>]*>"))); + painter.drawStaticText(1, h - text_rect.height() + 1, static_shadow); + painter.drawStaticText(0, h - text_rect.height(), static_text); + } + } + painter.end(); + } + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + require_update_texture_ = false; +} + +void OverlayTextDisplay::reset() +{ + RTDClass::reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void OverlayTextDisplay::processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) +{ + if (!isEnabled()) { + return; + } + if (!overlay_) { + static int count = 0; + std::stringstream ss; + ss << "OverlayTextDisplayObject" << count++; + overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_->show(); + } + if (overlay_) { + if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::DELETE) { + overlay_->hide(); + } else if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::ADD) { + overlay_->show(); + } + } + + // store message for update method + text_ = msg->text; + + if (!overtake_position_properties_) { + texture_width_ = msg->width; + texture_height_ = msg->height; + text_size_ = msg->text_size; + horizontal_dist_ = msg->horizontal_distance; + vertical_dist_ = msg->vertical_distance; + + horizontal_alignment_ = HorizontalAlignment{msg->horizontal_alignment}; + vertical_alignment_ = VerticalAlignment{msg->vertical_alignment}; + } + if (!overtake_bg_color_properties_) + bg_color_ = QColor( + msg->bg_color.r * 255.0, msg->bg_color.g * 255.0, msg->bg_color.b * 255.0, + msg->bg_color.a * 255.0); + if (!overtake_fg_color_properties_) { + fg_color_ = QColor( + msg->fg_color.r * 255.0, msg->fg_color.g * 255.0, msg->fg_color.b * 255.0, + msg->fg_color.a * 255.0); + font_ = msg->font; + line_width_ = msg->line_width; + } + if (overlay_) { + overlay_->setPosition( + horizontal_dist_, vertical_dist_, horizontal_alignment_, vertical_alignment_); + } + require_update_texture_ = true; +} + +void OverlayTextDisplay::updateOvertakePositionProperties() +{ + if (!overtake_position_properties_ && overtake_position_properties_property_->getBool()) { + updateVerticalDistance(); + updateHorizontalDistance(); + updateVerticalAlignment(); + updateHorizontalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + require_update_texture_ = true; + } + + overtake_position_properties_ = overtake_position_properties_property_->getBool(); + if (overtake_position_properties_) { + hor_dist_property_->show(); + ver_dist_property_->show(); + hor_alignment_property_->show(); + ver_alignment_property_->show(); + width_property_->show(); + height_property_->show(); + text_size_property_->show(); + } else { + hor_dist_property_->hide(); + ver_dist_property_->hide(); + hor_alignment_property_->hide(); + ver_alignment_property_->hide(); + width_property_->hide(); + height_property_->hide(); + text_size_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeFGColorProperties() +{ + if (!overtake_fg_color_properties_ && overtake_fg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateFGColor(); + updateFGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; + } + overtake_fg_color_properties_ = overtake_fg_color_properties_property_->getBool(); + if (overtake_fg_color_properties_) { + fg_color_property_->show(); + fg_alpha_property_->show(); + line_width_property_->show(); + font_property_->show(); + } else { + fg_color_property_->hide(); + fg_alpha_property_->hide(); + line_width_property_->hide(); + font_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeBGColorProperties() +{ + if (!overtake_bg_color_properties_ && overtake_bg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateBGColor(); + updateBGAlpha(); + require_update_texture_ = true; + } + overtake_bg_color_properties_ = overtake_bg_color_properties_property_->getBool(); + if (overtake_bg_color_properties_) { + bg_color_property_->show(); + bg_alpha_property_->show(); + } else { + bg_color_property_->hide(); + bg_alpha_property_->hide(); + } +} + +void OverlayTextDisplay::updateAlignBottom() +{ + if (align_bottom_ != align_bottom_property_->getBool()) { + require_update_texture_ = true; + } + align_bottom_ = align_bottom_property_->getBool(); +} + +void OverlayTextDisplay::updateInvertShadow() +{ + if (invert_shadow_ != invert_shadow_property_->getBool()) { + require_update_texture_ = true; + } + invert_shadow_ = invert_shadow_property_->getBool(); +} + +void OverlayTextDisplay::updateVerticalDistance() +{ + vertical_dist_ = ver_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalDistance() +{ + horizontal_dist_ = hor_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateVerticalAlignment() +{ + vertical_alignment_ = + VerticalAlignment{static_cast(ver_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalAlignment() +{ + horizontal_alignment_ = + HorizontalAlignment{static_cast(hor_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateWidth() +{ + texture_width_ = width_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHeight() +{ + texture_height_ = height_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateTextSize() +{ + text_size_ = text_size_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGColor() +{ + QColor c = bg_color_property_->getColor(); + bg_color_.setRed(c.red()); + bg_color_.setGreen(c.green()); + bg_color_.setBlue(c.blue()); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGAlpha() +{ + bg_color_.setAlpha(bg_alpha_property_->getFloat() * 255.0); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGColor() +{ + QColor c = fg_color_property_->getColor(); + fg_color_.setRed(c.red()); + fg_color_.setGreen(c.green()); + fg_color_.setBlue(c.blue()); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGAlpha() +{ + fg_color_.setAlpha(fg_alpha_property_->getFloat() * 255.0); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFont() +{ + int font_index = font_property_->getOptionInt(); + if (font_index < font_families_.size()) { + font_ = font_families_[font_index].toStdString(); + } else { + RVIZ_COMMON_LOG_ERROR_STREAM("Unexpected error at selecting font index " << font_index); + return; + } + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateLineWidth() +{ + line_width_ = line_width_property_->getInt(); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +} // namespace awf_2d_overlay_vehicle + +#include +PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::OverlayTextDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp new file mode 100644 index 0000000000000..e65a74415fb6a --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp @@ -0,0 +1,267 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_utils.hpp" + +#include + +namespace awf_2d_overlay_vehicle +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + if (mOverlayMgr) { + mOverlayMgr->destroyOverlayElement(panel_); + mOverlayMgr->destroy(overlay_); + } + if (panel_material_) { + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + } +} + +std::string OverlayObject::getName() const +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() const +{ + return texture_ != nullptr; +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] width=0 is specified as texture size"); + width = 1; + } + + if (height == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] height=0 is specified as texture size"); + height = 1; + } + + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment, + VerticalAlignment ver_alignment) +{ + // ogre position is always based on the top left corner of the panel, while our position input + // depends on the chosen alignment, i.e. if the horizontal alignment is right, increasing the + // horizontal dist moves the panel to the left (further away from the right border) + double left = 0; + double top = 0; + + switch (hor_alignment) { + case HorizontalAlignment::LEFT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_LEFT); + left = hor_dist; + break; + case HorizontalAlignment::CENTER: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_CENTER); + left = hor_dist - panel_->getWidth() / 2; + break; + case HorizontalAlignment::RIGHT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_RIGHT); + left = -hor_dist - panel_->getWidth(); + break; + } + + switch (ver_alignment) { + case VerticalAlignment::BOTTOM: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_BOTTOM); + top = -ver_dist - panel_->getHeight(); + break; + case VerticalAlignment::CENTER: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_CENTER); + top = ver_dist - panel_->getHeight() / 2; + break; + case VerticalAlignment::TOP: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_TOP); + top = ver_dist; + break; + } + + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() const +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() const +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() const +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp new file mode 100644 index 0000000000000..f2a82dd386b37 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp @@ -0,0 +1,501 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "signal_display.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SignalDisplay::SignalDisplay() +{ + property_width_ = new rviz_common::properties::IntProperty( + "Width", 517, "Width of the overlay", this, SLOT(updateOverlaySize())); + property_height_ = new rviz_common::properties::IntProperty( + "Height", 175, "Height of the overlay", this, SLOT(updateOverlaySize())); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 10, "Left position of the overlay", this, SLOT(updateOverlayPosition())); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 10, "Top position of the overlay", this, SLOT(updateOverlayPosition())); + property_signal_color_ = new rviz_common::properties::ColorProperty( + "Signal Color", QColor(94, 130, 255), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + + // Initialize the component displays + steering_wheel_display_ = std::make_unique(); + gear_display_ = std::make_unique(); + speed_display_ = std::make_unique(); + turn_signals_display_ = std::make_unique(); + traffic_display_ = std::make_unique(); + speed_limit_display_ = std::make_unique(); +} + +void SignalDisplay::onInitialize() +{ + std::lock_guard lock(property_mutex_); + + rviz_common::Display::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + static int count = 0; + std::stringstream ss; + ss << "SignalDisplayObject" << count++; + overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_->show(); + updateOverlaySize(); + updateOverlayPosition(); + + auto rviz_ros_node = context_->getRosNodeAbstraction(); + + gear_topic_property_ = std::make_unique( + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Topic for Gear Data", this, SLOT(topic_updated_gear())); + gear_topic_property_->initialize(rviz_ros_node); + + turn_signals_topic_property_ = std::make_unique( + "Turn Signals Topic", "/vehicle/status/turn_indicators_status", + "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + SLOT(topic_updated_turn_signals())); + turn_signals_topic_property_->initialize(rviz_ros_node); + + speed_topic_property_ = std::make_unique( + "Speed Topic", "/vehicle/status/velocity_status", + "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, + SLOT(topic_updated_speed())); + speed_topic_property_->initialize(rviz_ros_node); + + steering_topic_property_ = std::make_unique( + "Steering Topic", "/vehicle/status/steering_status", + "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, + SLOT(topic_updated_steering())); + steering_topic_property_->initialize(rviz_ros_node); + + hazard_lights_topic_property_ = std::make_unique( + "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", + "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + SLOT(topic_updated_hazard_lights())); + hazard_lights_topic_property_->initialize(rviz_ros_node); + + speed_limit_topic_property_ = std::make_unique( + "Speed Limit Topic", "/planning/scenario_planning/current_max_velocity", + "tier4_planning_msgs/msg/VelocityLimit", "Topic for Speed Limit Data", this, + SLOT(topic_updated_speed_limit())); + 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/msg/TrafficSignalArray", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); + traffic_topic_property_->initialize(rviz_ros_node); +} + +void SignalDisplay::setupRosSubscriptions() +{ + // Don't create a node, just use the one from the parent class + auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + gear_sub_ = rviz_node_->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); + + steering_sub_ = rviz_node_->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); + + speed_sub_ = rviz_node_->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); + + turn_signals_sub_ = + rviz_node_->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); + + hazard_lights_sub_ = + rviz_node_->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); + + traffic_sub_ = rviz_node_->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); + + speed_limit_sub_ = rviz_node_->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +SignalDisplay::~SignalDisplay() +{ + std::lock_guard lock(property_mutex_); + overlay_.reset(); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + traffic_sub_.reset(); + + steering_wheel_display_.reset(); + gear_display_.reset(); + speed_display_.reset(); + turn_signals_display_.reset(); + traffic_display_.reset(); + speed_limit_display_.reset(); + + gear_topic_property_.reset(); + turn_signals_topic_property_.reset(); + speed_topic_property_.reset(); + steering_topic_property_.reset(); + hazard_lights_topic_property_.reset(); + traffic_topic_property_.reset(); +} + +void SignalDisplay::update(float /* wall_dt */, float /* ros_dt */) +{ + if (!overlay_) { + return; + } + awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(Qt::transparent); + drawWidget(hud); +} + +void SignalDisplay::onEnable() +{ + std::lock_guard lock(property_mutex_); + if (overlay_) { + overlay_->show(); + } + setupRosSubscriptions(); +} + +void SignalDisplay::onDisable() +{ + std::lock_guard lock(property_mutex_); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void SignalDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (traffic_display_) { + traffic_display_->updateTrafficLightData(msg); + } +} + +void SignalDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_limit_display_) { + speed_limit_display_->updateSpeedLimitData(msg); + queueRender(); + } +} + +void SignalDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateHazardLightsData(msg); + queueRender(); + } +} + +void SignalDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (gear_display_) { + gear_display_->updateGearData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (steering_wheel_display_) { + steering_wheel_display_->updateSteeringData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_display_) { + speed_display_->updateSpeedData(msg); + queueRender(); + } +} + +void SignalDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateTurnSignalsData(msg); + queueRender(); + } +} + +void SignalDisplay::drawWidget(QImage & hud) +{ + std::lock_guard lock(property_mutex_); + + if (!overlay_->isVisible()) { + return; + } + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + QRectF backgroundRect(0, 0, 322, hud.height()); + drawBackground(painter, backgroundRect); + + // Draw components + if (steering_wheel_display_) { + steering_wheel_display_->drawSteeringWheel(painter, backgroundRect); + } + if (gear_display_) { + gear_display_->drawGearIndicator(painter, backgroundRect); + } + if (speed_display_) { + speed_display_->drawSpeedDisplay(painter, backgroundRect); + } + if (turn_signals_display_) { + turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor()); + } + + // a 27px space between the two halves of the HUD + + QRectF smallerBackgroundRect(349, 0, 168, hud.height() / 2); + + drawBackground(painter, smallerBackgroundRect); + + if (traffic_display_) { + traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect); + } + + if (speed_limit_display_) { + speed_limit_display_->drawSpeedLimitIndicator(painter, smallerBackgroundRect); + } + + painter.end(); +} + +void SignalDisplay::drawBackground(QPainter & painter, const QRectF & backgroundRect) +{ + painter.setBrush(QColor(0, 0, 0, 255 * 0.2)); // Black background with opacity + painter.setPen(Qt::NoPen); + painter.drawRoundedRect( + backgroundRect, backgroundRect.height() / 2, backgroundRect.height() / 2); // Circular ends +} + +void SignalDisplay::reset() +{ + rviz_common::Display::reset(); + overlay_->hide(); +} + +void SignalDisplay::updateOverlaySize() +{ + std::lock_guard lock(mutex_); + overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + queueRender(); +} + +void SignalDisplay::updateOverlayPosition() +{ + std::lock_guard lock(mutex_); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + queueRender(); +} + +void SignalDisplay::updateOverlayColor() +{ + std::lock_guard lock(mutex_); + queueRender(); +} + +void SignalDisplay::topic_updated_gear() +{ + // resubscribe to the topic + gear_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + gear_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); +} + +void SignalDisplay::topic_updated_steering() +{ + // resubscribe to the topic + steering_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + steering_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); +} + +void SignalDisplay::topic_updated_speed() +{ + // resubscribe to the topic + speed_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); +} + +void SignalDisplay::topic_updated_speed_limit() +{ + // resubscribe to the topic + speed_limit_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_limit_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +void SignalDisplay::topic_updated_turn_signals() +{ + // resubscribe to the topic + turn_signals_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + turn_signals_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); +} + +void SignalDisplay::topic_updated_hazard_lights() +{ + // resubscribe to the topic + hazard_lights_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + hazard_lights_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); +} + +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::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); +} + +} // namespace awf_2d_overlay_vehicle + +#include +PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::SignalDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp new file mode 100644 index 0000000000000..8212758c09a9f --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp @@ -0,0 +1,109 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "speed_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SpeedDisplay::SpeedDisplay() : current_speed_(0.0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in + float speed = msg->longitudinal_velocity; + // we received it as a m/s value, but we want to display it in km/h + current_speed_ = (speed * 3.6); + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +// void SpeedDisplay::processMessage(const +// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// { +// try +// { +// current_speed_ = std::round(msg->state.longitudinal_velocity_mps * 3.6); +// } +// catch (const std::exception &e) +// { +// std::cerr << "Error in processMessage: " << e.what() << std::endl; +// } +// } + +void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect) +{ + QFont referenceFont("Quicksand", 80, QFont::Bold); + painter.setFont(referenceFont); + QRect referenceRect = painter.fontMetrics().boundingRect("88"); + QPointF referencePos( + backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2); + + QString speedNumber = QString::number(current_speed_, 'f', 0); + int fontSize = 60; + QFont speedFont("Quicksand", fontSize); + painter.setFont(speedFont); + + // Calculate the bounding box of the speed number + QRect speedNumberRect = painter.fontMetrics().boundingRect(speedNumber); + + // Center the speed number in the backgroundRect + QPointF speedPos( + backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y()); + painter.setPen(gray); + painter.drawText(speedPos, speedNumber); + + QFont unitFont("Quicksand", 14); + painter.setFont(unitFont); + QString speedUnit = "km/h"; + QRect unitRect = painter.fontMetrics().boundingRect(speedUnit); + QPointF unitPos( + (backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height()); + painter.drawText(unitPos, speedUnit); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp new file mode 100644 index 0000000000000..fcc1df998798b --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp @@ -0,0 +1,105 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "speed_limit_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedLimitDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + current_limit = msg->max_velocity; +} + +void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + // #C2C2C2 + painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setBrush(QBrush(gray, Qt::SolidPattern)); + + // Define the area for the outer circle + QRectF outerCircleRect = backgroundRect; + outerCircleRect.setWidth(backgroundRect.width() / 2 - 20); + outerCircleRect.setHeight(backgroundRect.height() - 20); + outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 10, backgroundRect.top() + 10)); + + // Define the area for the inner circle + QRectF innerCircleRect = outerCircleRect; + innerCircleRect.setWidth(outerCircleRect.width() / 1.375); + innerCircleRect.setHeight(outerCircleRect.height() / 1.375); + innerCircleRect.moveCenter(outerCircleRect.center()); + + // Draw the outer circle + painter.drawEllipse(outerCircleRect); + + // Change the composition mode and draw the inner circle + painter.setCompositionMode(QPainter::CompositionMode_Clear); + painter.drawEllipse(innerCircleRect); + + // Reset the composition mode + painter.setCompositionMode(QPainter::CompositionMode_SourceOver); + + int current_limit_int = std::round(current_limit * 3.6); + + // Define the text to be drawn + QString text = QString::number(current_limit_int); + + // Set the font and color for the text + QFont font = QFont("Quicksand", 14, QFont::Bold); + + painter.setFont(font); + // #C2C2C2 + painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + // Draw the text in the center of the circle + painter.drawText(innerCircleRect, Qt::AlignCenter, text); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp new file mode 100644 index 0000000000000..b4da8ff5f8ffb --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp @@ -0,0 +1,123 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_wheel_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SteeringWheelDisplay::SteeringWheelDisplay() +{ + // Load the Quicksand font + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } + + // Load the wheel image + std::string image_path = package_path + "/assets/images/wheel.png"; + wheelImage.load(image_path.c_str()); + scaledWheelImage = wheelImage.scaled(54, 54, Qt::KeepAspectRatio, Qt::SmoothTransformation); +} + +void SteeringWheelDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->steering_angle is the field you're interested in + float steeringAngle = msg->steering_tire_angle; + // we received it as a radian value, but we want to display it in degrees + steering_angle_ = + (steeringAngle * -180 / M_PI) * + 17; // 17 is the ratio between the steering wheel and the steering tire angle i assume + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + QImage wheel = coloredImage(scaledWheelImage, gray); + + // Rotate the wheel + float steeringAngle = steering_angle_; // No need to round here + // Calculate the position + int wheelCenterX = backgroundRect.right() - wheel.width() - 17.5; + int wheelCenterY = backgroundRect.height() - wheel.height() + 15; + + // Rotate the wheel image + QTransform rotationTransform; + rotationTransform.translate(wheel.width() / 2.0, wheel.height() / 2.0); + rotationTransform.rotate(steeringAngle); + rotationTransform.translate(-wheel.width() / 2.0, -wheel.height() / 2.0); + + QImage rotatedWheel = wheel.transformed(rotationTransform, Qt::SmoothTransformation); + + QPointF drawPoint( + wheelCenterX - rotatedWheel.width() / 2, wheelCenterY - rotatedWheel.height() / 2); + + // Draw the rotated image + painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); + + QString steeringAngleStringAfterModulo = QString::number(fmod(steeringAngle, 360), 'f', 0); + + // Draw the steering angle text + QFont steeringFont("Quicksand", 9, QFont::Bold); + painter.setFont(steeringFont); + painter.setPen(QColor(0, 0, 0, 255)); + QRect steeringRect( + wheelCenterX - wheelImage.width() / 2, wheelCenterY - wheelImage.height() / 2, + wheelImage.width(), wheelImage.height()); + painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleStringAfterModulo + "°"); +} + +QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp new file mode 100644 index 0000000000000..233da1f36b4a7 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp @@ -0,0 +1,113 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "traffic_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +TrafficDisplay::TrafficDisplay() +{ + // Load the traffic light image + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string image_path = package_path + "/assets/images/traffic.png"; + traffic_light_image_.load(image_path.c_str()); +} + +void TrafficDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg) +{ + current_traffic_ = *msg; +} + +void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + // Define the area for the circle (background) + QRectF circleRect = backgroundRect; + circleRect.setWidth(backgroundRect.width() / 2 - 20); + circleRect.setHeight(backgroundRect.height() - 20); + circleRect.moveTopRight(QPointF(backgroundRect.right() - 10, backgroundRect.top() + 10)); + + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + + // Define the area for the traffic light image (should be smaller or positioned within the circle) + QRectF imageRect = + circleRect.adjusted(15, 15, -15, -15); // Adjusting the rectangle to make the image smaller + + QImage scaled_traffic_image = traffic_light_image_.scaled( + imageRect.size().toSize(), Qt::KeepAspectRatio, Qt::SmoothTransformation); + + if (current_traffic_.signals.size() > 0) { + switch (current_traffic_.signals[0].elements[0].color) { + case 1: + painter.setBrush(QBrush(red)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 2: + painter.setBrush(QBrush(yellow)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 3: + painter.setBrush(QBrush(green)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 4: + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + default: + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + } + } + // make the image thicker + painter.setPen(QPen(Qt::black, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + painter.drawImage(imageRect, scaled_traffic_image); +} + +QImage TrafficDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp new file mode 100644 index 0000000000000..a9b780cb4eab2 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "turn_signals_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) +{ + last_toggle_time_ = std::chrono::steady_clock::now(); + + // Load the arrow image + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string image_path = package_path + "/assets/images/arrow.png"; + arrowImage.load(image_path.c_str()); +} + +void TurnSignalsDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_turn_signal_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_hazard_lights_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::drawArrows( + QPainter & painter, const QRectF & backgroundRect, const QColor & color) +{ + QImage scaledLeftArrow = arrowImage.scaled(64, 43, Qt::KeepAspectRatio, Qt::SmoothTransformation); + scaledLeftArrow = coloredImage(scaledLeftArrow, gray); + QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false); + int arrowYPos = (backgroundRect.height() / 3 - scaledLeftArrow.height() / 2); + int leftArrowXPos = backgroundRect.width() / 4 - scaledLeftArrow.width(); // Adjust as needed + int rightArrowXPos = backgroundRect.width() * 3 / 4; // Adjust as needed + + bool leftActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + bool rightActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + + // Color the arrows based on the state of the turn signals and hazard lights by having them blink + // on and off + if (this->blink_on_) { + if (leftActive) { + scaledLeftArrow = coloredImage(scaledLeftArrow, color); + } + if (rightActive) { + scaledRightArrow = coloredImage(scaledRightArrow, color); + } + } + + // Draw the arrows + painter.drawImage(QPointF(leftArrowXPos, arrowYPos), scaledLeftArrow); + painter.drawImage(QPointF(rightArrowXPos, arrowYPos), scaledRightArrow); + + auto now = std::chrono::steady_clock::now(); + if ( + std::chrono::duration_cast(now - last_toggle_time_) >= + blink_interval_) { + blink_on_ = !blink_on_; // Toggle the blink state + last_toggle_time_ = now; + } +} + +QImage TurnSignalsDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst new file mode 100644 index 0000000000000..e7672ee001955 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst @@ -0,0 +1,24 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rviz_2d_overlay_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.3.0 (2023-05-18) +------------------ +* Removed old position message fields +* Contributors: Dominik, Jonas Otto + +1.2.1 (2022-09-30) +------------------ + +1.2.0 (2022-09-27) +------------------ +* Rename package from overlay_rviz_msgs to rviz_2d_overlay_msgs +* Contributors: Jonas Otto + +1.1.0 (2022-09-11) +------------------ + +1.0.0 (2022-08-30) +------------------ +* Create OverlayText message (currently same as jsk_rviz_plugins) +* Contributors: Jonas Otto, Dominik Authaler diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt new file mode 100644 index 0000000000000..9e9a8f277fd53 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(rviz_2d_overlay_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif () + + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/OverlayText.msg" + DEPENDENCIES + std_msgs +) + +ament_package() diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg new file mode 100644 index 0000000000000..db3cf628b895b --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg @@ -0,0 +1,31 @@ +uint8 ADD = 0 +uint8 DELETE = 1 + +# constants for the horizontal and vertical alignment +uint8 LEFT = 0 +uint8 RIGHT = 1 +uint8 CENTER = 2 +uint8 TOP = 3 +uint8 BOTTOM = 4 + +uint8 action + +int32 width +int32 height +# Position: Positive values move the overlay towards the center of the window, +# for center alignment positive values move the overlay towards the bottom right +int32 horizontal_distance # Horizontal distance from left/right border or center, depending on alignment +int32 vertical_distance # Vertical distance between from top/bottom border or center, depending on alignment + +# Alignment of the overlay withing RVIZ +uint8 horizontal_alignment # one of LEFT, CENTER, RIGHT +uint8 vertical_alignment # one of TOP, CENTER, BOTTOM + +std_msgs/ColorRGBA bg_color + +int32 line_width +float32 text_size +string font +std_msgs/ColorRGBA fg_color + +string text diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml new file mode 100644 index 0000000000000..53396c64aa156 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml @@ -0,0 +1,24 @@ + + + + rviz_2d_overlay_msgs + 1.3.0 + Messages describing 2D overlays for RVIZ, extracted/derived from the jsk_visualization package. + Team Spatzenhirn + BSD-3-Clause + + ament_cmake + rosidl_default_generators + + autoware_auto_vehicle_msgs + autoware_perception_msgs + std_msgs + unique_identifier_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 2f2472515ebad..796e276c376d6 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -6,8 +6,8 @@ The object_recognition_utils package Takayuki Murooka Satoshi Tanaka - Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 5304d481737e5..9d5fbf40a4e89 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -5,8 +5,8 @@ 0.1.0 The perception_utils package Satoshi Tanaka - Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/tensorrt_common/include/tensorrt_common/logger.hpp b/common/tensorrt_common/include/tensorrt_common/logger.hpp index 98fe18794998d..355efe5167885 100644 --- a/common/tensorrt_common/include/tensorrt_common/logger.hpp +++ b/common/tensorrt_common/include/tensorrt_common/logger.hpp @@ -19,6 +19,7 @@ #include "NvInferRuntimeCommon.h" +#include #include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include namespace tensorrt_common { @@ -200,7 +202,15 @@ class Logger : public nvinfer1::ILogger // NOLINT public: // Logger(Severity severity = Severity::kWARNING) // Logger(Severity severity = Severity::kVERBOSE) - explicit Logger(Severity severity = Severity::kINFO) : mReportableSeverity(severity) {} + explicit Logger(Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(true), mThrottleStopFlag(false) + { + } + + explicit Logger(const bool verbose, Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(verbose), mThrottleStopFlag(false) + { + } //! //! \enum TestResult @@ -234,7 +244,44 @@ class Logger : public nvinfer1::ILogger // NOLINT //! void log(Severity severity, const char * msg) noexcept override { - LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + if (mVerbose) { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + } + + /** + * @brief Logging with throttle. + * + * @example + * Logger logger(); + * auto log_thread = logger.log_throttle(nvinfer1::ILogger::Severity::kINFO, "SOME MSG", 1); + * // some operation + * logger.stop_throttle(log_thread); + * + * @param severity + * @param msg + * @param duration + * @return std::thread + * + */ + std::thread log_throttle(Severity severity, const char * msg, const int duration) noexcept + { + mThrottleStopFlag.store(false); + auto log_func = [this](Severity s, const char * m, const int d) { + while (!mThrottleStopFlag.load()) { + this->log(s, m); + std::this_thread::sleep_for(std::chrono::seconds(d)); + } + }; + + std::thread log_thread(log_func, severity, msg, duration); + return log_thread; + } + + void stop_throttle(std::thread & log_thread) noexcept + { + mThrottleStopFlag.store(true); + log_thread.join(); } //! @@ -430,6 +477,8 @@ class Logger : public nvinfer1::ILogger // NOLINT } Severity mReportableSeverity; + bool mVerbose; + std::atomic mThrottleStopFlag; }; namespace diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index f1e4835d2ba2d..2b218cd3e49f2 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -186,7 +186,11 @@ void TrtCommon::setup() } else { std::cout << "Building... " << cache_engine_path << std::endl; logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); buildEngineFromOnnx(model_file_path_, cache_engine_path); + logger_.stop_throttle(log_thread); logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); } engine_path = cache_engine_path; diff --git a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt index c6d4e30061626..cdc57743a6cb1 100644 --- a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt +++ b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt @@ -11,6 +11,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED + src/automatic_checkpoint_append_tool.cpp src/automatic_goal_sender.cpp src/automatic_goal_append_tool.cpp src/automatic_goal_panel.cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml index deda078ace22e..fb5331379acb6 100644 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -6,6 +6,8 @@ The autoware automatic goal rviz plugin package Shumpei Wakabayashi Dawid Moszyński + Kyoichi Sugahara + Satoshi Ota Apache License 2.0 Dawid Moszyński @@ -22,6 +24,7 @@ rviz_default_plugins tf2 tf2_geometry_msgs + tier4_autoware_utils visualization_msgs yaml-cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml index e7d5224550868..e9d77491941ed 100644 --- a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml @@ -9,4 +9,9 @@ base_class_type="rviz_common::Tool"> AutowareAutomaticGoalTool + + AutowareAutomaticCheckpointTool + diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp new file mode 100644 index 0000000000000..4efa6fedbaabd --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_checkpoint_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticCheckpointTool::AutowareAutomaticCheckpointTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/checkpoint", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticCheckpointTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendCheckpoint"); + updateTopic(); +} + +void AutowareAutomaticCheckpointTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticCheckpointTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticCheckpointTool"), + "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, position_z_->getFloat(), theta, + fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticCheckpointTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp new file mode 100644 index 0000000000000..4ea3fa4bd009a --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp @@ -0,0 +1,91 @@ +// 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ +#define AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticCheckpointTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticCheckpointTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp index bee390bfe29ed..6b8d7765a989a 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -16,6 +16,8 @@ #include "automatic_goal_panel.hpp" +#include + namespace rviz_plugins { AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) @@ -139,6 +141,9 @@ void AutowareAutomaticGoalPanel::onInitialize() sub_append_goal_ = raw_node_->create_subscription( "~/automatic_goal/goal", 5, std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); + sub_append_checkpoint_ = raw_node_->create_subscription( + "~/automatic_goal/checkpoint", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendCheckpoint, this, std::placeholders::_1)); initCommunication(raw_node_.get()); } @@ -244,12 +249,28 @@ void AutowareAutomaticGoalPanel::onClickSaveListToFile() void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) { if (state_ == State::EDITING) { - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); updateGoalsList(); updateGUI(); } } +void AutowareAutomaticGoalPanel::onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose) +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList before set checkpoint"); + return; + } + + current_goal_ = goals_list_widget_ptr_->currentRow(); + if (current_goal_ >= goals_list_.size()) { + return; + } + + goals_list_.at(current_goal_).checkpoint_pose_ptrs.push_back(pose); + publishMarkers(); +} + // Override void AutowareAutomaticGoalPanel::onCallResult() { @@ -418,47 +439,71 @@ void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const void AutowareAutomaticGoalPanel::publishMarkers() { + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + MarkerArray text_array; MarkerArray arrow_array; // Clear existing - visualization_msgs::msg::Marker marker; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = 0; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - text_array.markers.push_back(marker); - marker.ns = "poses"; - arrow_array.markers.push_back(marker); - pub_marker_->publish(text_array); - pub_marker_->publish(arrow_array); + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::DELETEALL; + text_array.markers.push_back(marker); + pub_marker_->publish(text_array); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + arrow_array.markers.push_back(marker); + pub_marker_->publish(arrow_array); + } + text_array.markers.clear(); arrow_array.markers.clear(); - // Publish current - for (unsigned i = 0; i < goals_list_.size(); i++) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = static_cast(i); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + + const auto push_arrow_marker = [&](const auto & pose, const auto & color, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", id, Marker::ARROW, + createMarkerScale(1.6, 0.5, 0.5), color); marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = goals_list_[i]->pose; + marker.pose = pose; marker.lifetime = rclcpp::Duration(0, 0); - marker.scale.x = 1.6; - marker.scale.y = 1.6; - marker.scale.z = 1.6; - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.color.a = 1.0; marker.frame_locked = false; - marker.text = "G" + std::to_string(i); - text_array.markers.push_back(marker); - marker.ns = "poses"; - marker.scale.y = 0.2; - marker.scale.z = 0.2; - marker.type = visualization_msgs::msg::Marker::ARROW; arrow_array.markers.push_back(marker); + }; + + const auto push_text_marker = [&](const auto & pose, const auto & text, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(1.6, 1.6, 1.6), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = false; + marker.text = text; + text_array.markers.push_back(marker); + }; + + // Publish current + size_t id = 0; + for (size_t i = 0; i < goals_list_.size(); ++i) { + { + const auto pose = goals_list_.at(i).goal_pose_ptr->pose; + push_arrow_marker(pose, createMarkerColor(0.0, 1.0, 0.0, 0.999), id++); + push_text_marker(pose, "Goal:" + std::to_string(i), id++); + } + + for (size_t j = 0; j < goals_list_.at(i).checkpoint_pose_ptrs.size(); ++j) { + const auto pose = goals_list_.at(i).checkpoint_pose_ptrs.at(j)->pose; + push_arrow_marker(pose, createMarkerColor(1.0, 1.0, 0.0, 0.999), id++); + push_text_marker( + pose, "Checkpoint:" + std::to_string(i) + "[Goal:" + std::to_string(j) + "]", id++); + } } pub_marker_->publish(text_array); pub_marker_->publish(arrow_array); @@ -469,13 +514,13 @@ void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) { YAML::Node node; for (unsigned i = 0; i < goals_list_.size(); ++i) { - node[i]["position_x"] = goals_list_[i]->pose.position.x; - node[i]["position_y"] = goals_list_[i]->pose.position.y; - node[i]["position_z"] = goals_list_[i]->pose.position.z; - node[i]["orientation_x"] = goals_list_[i]->pose.orientation.x; - node[i]["orientation_y"] = goals_list_[i]->pose.orientation.y; - node[i]["orientation_z"] = goals_list_[i]->pose.orientation.z; - node[i]["orientation_w"] = goals_list_[i]->pose.orientation.w; + node[i]["position_x"] = goals_list_[i].goal_pose_ptr->pose.position.x; + node[i]["position_y"] = goals_list_[i].goal_pose_ptr->pose.position.y; + node[i]["position_z"] = goals_list_[i].goal_pose_ptr->pose.position.z; + node[i]["orientation_x"] = goals_list_[i].goal_pose_ptr->pose.orientation.x; + node[i]["orientation_y"] = goals_list_[i].goal_pose_ptr->pose.orientation.y; + node[i]["orientation_z"] = goals_list_[i].goal_pose_ptr->pose.orientation.z; + node[i]["orientation_w"] = goals_list_[i].goal_pose_ptr->pose.orientation.w; } std::ofstream file_out(file_path); file_out << node; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp index 0ec9ca530f074..72a5bd4fb80fe 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp @@ -61,7 +61,9 @@ class AutowareAutomaticGoalPanel : public rviz_common::Panel, public automatic_goal::AutowareAutomaticGoalSender { using State = automatic_goal::AutomaticGoalState; + using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; + using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; @@ -96,6 +98,7 @@ public Q_SLOTS: // NOLINT for Qt // Inputs void onAppendGoal(const PoseStamped::ConstSharedPtr pose); + void onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose); // Visual updates void updateGUI(); @@ -116,6 +119,7 @@ public Q_SLOTS: // NOLINT for Qt // Pub/Sub rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_checkpoint_{nullptr}; // Containers rclcpp::Node::SharedPtr raw_node_{nullptr}; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp index cc8d46e2f60c6..3ca368a3bd1a4 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -108,9 +108,9 @@ void AutowareAutomaticGoalSender::updateGoalsList() std::stringstream ss; ss << std::fixed << std::setprecision(2); tf2::Quaternion tf2_quat; - tf2::convert(goal->pose.orientation, tf2_quat); - ss << "G" << i << " (" << goal->pose.position.x << ", "; - ss << goal->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; + tf2::convert(goal.goal_pose_ptr->pose.orientation, tf2_quat); + ss << "G" << i << " (" << goal.goal_pose_ptr->pose.position.x << ", "; + ss << goal.goal_pose_ptr->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); } onGoalListUpdated(); @@ -178,7 +178,7 @@ void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) pose->pose.orientation.y = goal["orientation_y"].as(); pose->pose.orientation.z = goal["orientation_z"].as(); pose->pose.orientation.w = goal["orientation_w"].as(); - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); } resetAchievedGoals(); updateGoalsList(); diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp index aacdada352811..88b7b5e7dac20 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp @@ -80,8 +80,11 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } auto req = std::make_shared(); - req->header = goals_list_.at(goal_index)->header; - req->goal = goals_list_.at(goal_index)->pose; + req->header = goals_list_.at(goal_index).goal_pose_ptr->header; + req->goal = goals_list_.at(goal_index).goal_pose_ptr->pose; + for (const auto & checkpoint : goals_list_.at(goal_index).checkpoint_pose_ptrs) { + req->waypoints.push_back(checkpoint->pose); + } client->async_send_request( req, [this](typename rclcpp::Client::SharedFuture result) { if (result.get()->status.code != 0) state_ = State::ERROR; @@ -120,6 +123,13 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } } + struct Route + { + explicit Route(const PoseStamped::ConstSharedPtr & goal) : goal_pose_ptr{goal} {} + PoseStamped::ConstSharedPtr goal_pose_ptr{}; + std::vector checkpoint_pose_ptrs{}; + }; + // Update void updateGoalsList(); virtual void updateAutoExecutionTimerTick(); @@ -155,7 +165,7 @@ class AutowareAutomaticGoalSender : public rclcpp::Node // Containers unsigned current_goal_{0}; State state_{State::INITIALIZING}; - std::vector goals_list_{}; + std::vector goals_list_{}; std::map> goals_achieved_{}; std::string goals_achieved_file_path_{}; diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index c46c2d237b605..faf254708ddbb 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -7,6 +7,9 @@ Kyoichi Sugahara Takamasa Horibe Makoto Kurihara + Mamoru Sobue + Takayuki Murooka + Apache License 2.0 Kyoichi Sugahara diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp index d0d8d61c4c231..53265872bffa8 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -125,7 +125,7 @@ Stat calcTrajectoryLength(const Trajectory & traj) Stat calcTrajectoryDuration(const Trajectory & traj) { double duration = 0.0; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double length = calcDistance2d(traj.points.at(i), traj.points.at(i + 1)); const double velocity = traj.points.at(i).longitudinal_velocity_mps; if (velocity != 0) { @@ -158,7 +158,7 @@ Stat calcTrajectoryAcceleration(const Trajectory & traj) Stat calcTrajectoryJerk(const Trajectory & traj) { Stat stat; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double vel = traj.points.at(i).longitudinal_velocity_mps; if (vel != 0) { const double duration = diff --git a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..4c1e8cf58c262 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_metrics_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets Charts) +set(QT_WIDGETS_LIB Qt5::Widgets) +set(QT_CHARTS_LIB Qt5::Charts) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/metrics_visualize_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_WIDGETS_LIB} + ${QT_CHARTS_LIB} +) + +target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic) +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/evaluator/tier4_metrics_rviz_plugin/README.md b/evaluator/tier4_metrics_rviz_plugin/README.md new file mode 100644 index 0000000000000..be94141254030 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/README.md @@ -0,0 +1,16 @@ +# tier4_metrics_rviz_plugin + +## Purpose + +This plugin panel to visualize `planning_evaluator` output. + +## Inputs / Outputs + +| Name | Type | Description | +| ---------------------------------------- | --------------------------------------- | ------------------------------------- | +| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output | + +## HowToUse + +1. Start rviz and select panels/Add new panel. +2. Select MetricsVisualizePanel and press OK. diff --git a/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png b/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png differ diff --git a/evaluator/tier4_metrics_rviz_plugin/package.xml b/evaluator/tier4_metrics_rviz_plugin/package.xml new file mode 100644 index 0000000000000..7bd930f4f3c4c --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + tier4_metrics_rviz_plugin + 0.0.0 + The tier4_metrics_rviz_plugin package + Satoshi OTA + Kyoichi Sugahara + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..5aca5bd7faa54 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,5 @@ + + + MetricsVisualizePanel + + diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp new file mode 100644 index 0000000000000..79da02b9b65c6 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -0,0 +1,86 @@ +// 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. +// + +#include "metrics_visualize_panel.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) +: rviz_common::Panel(parent), grid_(new QGridLayout()) +{ + setLayout(grid_); +} + +void MetricsVisualizePanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_ = raw_node_->create_subscription( + "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1}, + std::bind(&MetricsVisualizePanel::onMetrics, this, _1)); + + const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); + timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); +} + +void MetricsVisualizePanel::onTimer() +{ + std::lock_guard message_lock(mutex_); + + for (auto & [name, metric] : metrics_) { + metric.updateGraph(); + metric.updateTable(); + } +} + +void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg) +{ + std::lock_guard message_lock(mutex_); + + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + + constexpr size_t GRAPH_COL_SIZE = 5; + for (size_t i = 0; i < msg->status.size(); ++i) { + const auto & status = msg->status.at(i); + + if (metrics_.count(status.name) == 0) { + auto metric = Metric(status); + metrics_.emplace(status.name, metric); + grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false); + grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true); + grid_->setColumnStretch(i % GRAPH_COL_SIZE, true); + } + + metrics_.at(status.name).updateData(time, status); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel) diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp new file mode 100644 index 0000000000000..6708ecd7071e9 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp @@ -0,0 +1,205 @@ +// 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 METRICS_VISUALIZE_PANEL_HPP_ +#define METRICS_VISUALIZE_PANEL_HPP_ + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include + +#include + +#include +#include +#include + +namespace rviz_plugins +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using QtCharts::QChart; +using QtCharts::QChartView; +using QtCharts::QLineSeries; + +struct Metric +{ +public: + explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget) + { + init(status); + } + + void init(const DiagnosticStatus & status) + { + QStringList header{}; + + { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + label->setText("metric_name"); + labels.emplace("metric_name", label); + + header.push_back(QString::fromStdString(status.name)); + } + + for (const auto & [key, value] : status.values) { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + labels.emplace(key, label); + + auto plot = new QLineSeries; + plot->setName(QString::fromStdString(key)); + plots.emplace(key, plot); + chart->chart()->addSeries(plot); + chart->chart()->createDefaultAxes(); + + header.push_back(QString::fromStdString(key)); + } + + { + chart->chart()->setTitle(QString::fromStdString(status.name)); + chart->chart()->legend()->setVisible(true); + chart->chart()->legend()->detachFromChart(); + chart->setRenderHint(QPainter::Antialiasing); + } + + { + table->setColumnCount(status.values.size() + 1); + table->setHorizontalHeaderLabels(header); + table->verticalHeader()->hide(); + table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + table->setRowCount(1); + table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); + } + } + + void updateData(const double time, const DiagnosticStatus & status) + { + for (const auto & [key, value] : status.values) { + const double data = std::stod(value); + labels.at(key)->setText(QString::fromStdString(toString(data))); + plots.at(key)->append(time, data); + updateMinMax(data); + } + + { + const auto area = chart->chart()->plotArea(); + const auto rect = chart->chart()->legend()->rect(); + chart->chart()->legend()->setGeometry( + QRectF(area.x(), area.y(), area.width(), rect.height())); + chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time); + } + + { + table->setCellWidget(0, 0, labels.at("metric_name")); + } + + for (size_t i = 0; i < status.values.size(); ++i) { + table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key)); + } + } + + void updateMinMax(double data) + { + if (data < y_range_min) { + y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data; + chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min); + } + + if (data > y_range_max) { + y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data; + chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max); + } + } + + void updateTable() { table->update(); } + + void updateGraph() { chart->update(); } + + QChartView * getChartView() const { return chart; } + + QTableWidget * getTable() const { return table; } + +private: + static std::optional getValue(const DiagnosticStatus & status, std::string && key) + { + const auto itr = std::find_if( + status.values.begin(), status.values.end(), + [&](const auto & value) { return value.key == key; }); + + if (itr == status.values.end()) { + return std::nullopt; + } + + return itr->value; + } + + static std::string toString(const double & value) + { + std::stringstream ss; + ss << std::scientific << std::setprecision(2) << value; + return ss.str(); + } + + QChartView * chart; + QTableWidget * table; + + std::unordered_map labels; + std::unordered_map plots; + + double y_range_min{std::numeric_limits::max()}; + double y_range_max{std::numeric_limits::lowest()}; +}; + +class MetricsVisualizePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit MetricsVisualizePanel(QWidget * parent = nullptr); + void onInitialize() override; + +private Q_SLOTS: + +private: + rclcpp::Node::SharedPtr raw_node_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_; + + void onTimer(); + void onMetrics(const DiagnosticArray::ConstSharedPtr msg); + + QGridLayout * grid_; + + std::mutex mutex_; + std::unordered_map metrics_; +}; +} // namespace rviz_plugins + +#endif // METRICS_VISUALIZE_PANEL_HPP_ diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index f4d6679291849..9f51f7e6ea7a1 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -1,10 +1,10 @@ - - + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index ac7589ea2273b..98f5fcab7c91a 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 02c6da20e17da..c00c90d467090 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -1,152 +1,116 @@ + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 23a1201a84958..bdea584bd58ae 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -27,10 +27,10 @@ - + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py deleted file mode 100644 index 22a45fe7b8530..0000000000000 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ /dev/null @@ -1,136 +0,0 @@ -# Copyright 2020 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. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -import yaml - - -def launch_setup(context, *args, **kwargs): - # https://github.com/ros2/launch_ros/issues/156 - def load_composable_node_param(param_path): - with open(LaunchConfiguration(param_path).perform(context), "r") as f: - return yaml.safe_load(f)["/**"]["ros__parameters"] - - crop_box_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="crop_box_filter_measurement_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "measurement_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("crop_box_filter_measurement_range_param_path"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - voxel_grid_downsample_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name="voxel_grid_downsample_filter", - remappings=[ - ("input", "measurement_range/pointcloud"), - ("output", "voxel_grid_downsample/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_downsample_filter_param_path")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - random_downsample_component = ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent", - name="random_downsample_filter", - remappings=[ - ("input", "voxel_grid_downsample/pointcloud"), - ("output", LaunchConfiguration("output/pointcloud")), - ], - parameters=[load_composable_node_param("random_downsample_filter_param_path")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - composable_nodes = [ - crop_box_component, - voxel_grid_downsample_component, - random_downsample_component, - ] - - load_composable_nodes = LoadComposableNodes( - composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("lidar_container_name"), - ) - - return [load_composable_nodes] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - arg = DeclareLaunchArgument(name, default_value=default_value, description=description) - launch_arguments.append(arg) - - add_launch_arg( - "crop_box_filter_measurement_range_param_path", - [ - LaunchConfiguration("crop_box_filter_measurement_range_param_path"), - ], - "path to the parameter file of crop_box_filter_measurement_range", - ) - add_launch_arg( - "voxel_grid_downsample_filter_param_path", - [ - LaunchConfiguration("voxel_grid_downsample_filter_param_path"), - ], - "path to the parameter file of voxel_grid_downsample_filter", - ) - add_launch_arg( - "random_downsample_filter_param_path", - [ - LaunchConfiguration("random_downsample_filter_param_path"), - ], - "path to the parameter file of random_downsample_filter", - ) - add_launch_arg("use_intra_process", "true", "use ROS 2 component container communication") - add_launch_arg( - "lidar_container_name", - "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container", - "container name of main lidar used for localization", - ) - - add_launch_arg( - "output/pointcloud", - "downsample/pointcloud", - "final output topic name", - ) - add_launch_arg( - "output_measurement_range_sensor_points_topic", - "measurement_range/pointcloud", - "output topic name for crop box filter", - ) - add_launch_arg( - "output_voxel_grid_downsample_sensor_points_topic", - "voxel_grid_downsample/pointcloud", - "output topic name for voxel grid downsample filter", - ) - add_launch_arg( - "output_downsample_sensor_points_topic", - "downsample/pointcloud", - "output topic name for downsample filter. this is final output", - ) - - return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml new file mode 100644 index 0000000000000..4d0f7505d16a6 --- /dev/null +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 9b4f727c9ce52..a0a1cc1f6df64 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -27,6 +27,7 @@ gyro_odometer ndt_scan_matcher pointcloud_preprocessor + pose_estimator_arbiter pose_initializer pose_instability_detector topic_tools diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py deleted file mode 100644 index 23bd2fc337c97..0000000000000 --- a/launch/tier4_map_launch/launch/map.launch.py +++ /dev/null @@ -1,223 +0,0 @@ -# Copyright 2021 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. - -import os - -from ament_index_python import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.launch_description_sources import AnyLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def launch_setup(context, *args, **kwargs): - lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( - context - ) - pointcloud_map_loader_param_path = LaunchConfiguration( - "pointcloud_map_loader_param_path" - ).perform(context) - - with open(lanelet2_map_loader_param_path, "r") as f: - lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(pointcloud_map_loader_param_path, "r") as f: - pointcloud_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - map_hash_generator = Node( - package="map_loader", - executable="map_hash_generator", - name="map_hash_generator", - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - "pointcloud_map_path": LaunchConfiguration("pointcloud_map_path"), - } - ], - ) - - lanelet2_map_loader = ComposableNode( - package="map_loader", - plugin="Lanelet2MapLoaderNode", - name="lanelet2_map_loader", - remappings=[ - ("output/lanelet2_map", "vector_map"), - ], - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }, - lanelet2_map_loader_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - lanelet2_map_visualization = ComposableNode( - package="map_loader", - plugin="Lanelet2MapVisualizationNode", - name="lanelet2_map_visualization", - remappings=[ - ("input/lanelet2_map", "vector_map"), - ("output/lanelet2_map_marker", "vector_map_marker"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - pointcloud_map_loader = ComposableNode( - package="map_loader", - plugin="PointCloudMapLoaderNode", - name="pointcloud_map_loader", - remappings=[ - ("output/pointcloud_map", "pointcloud_map"), - ("output/pointcloud_map_metadata", "pointcloud_map_metadata"), - ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"), - ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"), - ("service/get_selected_pcd_map", "/map/get_selected_pointcloud_map"), - ], - parameters=[ - {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, - {"pcd_metadata_path": [LaunchConfiguration("pointcloud_map_metadata_path")]}, - pointcloud_map_loader_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - map_tf_generator = ComposableNode( - package="map_tf_generator", - plugin="VectorMapTFGeneratorNode", - name="vector_map_tf_generator", - parameters=[ - { - "map_frame": "map", - "viewer_frame": "viewer", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - map_projection_loader_launch_file = os.path.join( - get_package_share_directory("map_projection_loader"), - "launch", - "map_projection_loader.launch.xml", - ) - map_projection_loader = IncludeLaunchDescription( - AnyLaunchDescriptionSource(map_projection_loader_launch_file), - launch_arguments={ - "map_projector_info_path": LaunchConfiguration("map_projector_info_path"), - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }.items(), - ) - - container = ComposableNodeContainer( - name="map_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - lanelet2_map_loader, - lanelet2_map_visualization, - pointcloud_map_loader, - map_tf_generator, - ], - output="screen", - ) - - group = GroupAction( - [ - PushRosNamespace("map"), - container, - map_hash_generator, - map_projection_loader, - ] - ) - - return [group] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("map_path", "", "path to map directory"), - add_launch_arg( - "lanelet2_map_path", - [LaunchConfiguration("map_path"), "/lanelet2_map.osm"], - "path to lanelet2 map file", - ), - add_launch_arg( - "pointcloud_map_path", - [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"], - "path to pointcloud map file", - ), - add_launch_arg( - "pointcloud_map_metadata_path", - [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], - "path to pointcloud map metadata file", - ), - add_launch_arg( - "map_projector_info_path", - [LaunchConfiguration("map_path"), "/map_projector_info.yaml"], - "path to map projector info yaml file", - ), - add_launch_arg( - "lanelet2_map_loader_param_path", - [ - FindPackageShare("map_loader"), - "/config/lanelet2_map_loader.param.yaml", - ], - "path to lanelet2_map_loader param file", - ), - add_launch_arg( - "pointcloud_map_loader_param_path", - None, - "path to pointcloud_map_loader param file", - ), - add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication"), - add_launch_arg("use_multithread", "false", "use multithread"), - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [ - set_container_executable, - set_container_mt_executable, - ] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 26b2d462dd53f..132d1ec9be3ea 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,26 +1,68 @@ + + + + + + + + + + + - - - + + + + - - - - - - - - + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index f75a181bfb659..8fb41076204a8 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -8,6 +8,11 @@ Ryu Yamamoto Koji Minoda Kento Yabuuchi + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Masahiro Sakamoto Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml deleted file mode 100644 index ba86bc1e334ff..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ /dev/null @@ -1,437 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 885fa80ed7004..50b73709cc902 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -3,7 +3,7 @@ - + @@ -31,12 +31,11 @@ - - - + - + + @@ -57,23 +56,60 @@ - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + @@ -94,50 +130,116 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + + + + + + + + + + + + + + + + + - + - - - + + + + + + + + + + + + + - + - + + + + + - + - - + + + + + + - + + - + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml similarity index 57% rename from launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 4cb648852a03c..2232feb6d7c67 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -1,17 +1,13 @@ - - - - + - - - - + + + @@ -54,6 +50,47 @@ --> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -192,190 +229,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 new file mode 100644 index 0000000000000..34f0a01d0a688 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 0000000000000..cec0c3bc05aac --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml similarity index 72% rename from launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml index 26e7af07646cd..b09684281d33a 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml @@ -2,10 +2,9 @@ - - - - + + + @@ -14,33 +13,32 @@ - - + - + - + - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml deleted file mode 100644 index adedc2312677f..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 new file mode 100644 index 0000000000000..2f62e83ae0ef5 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 0000000000000..b4d19c9052a63 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -0,0 +1,154 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml similarity index 51% rename from launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index 04c4264b70e5f..6ccff44933966 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -1,113 +1,19 @@ - - + - - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - + + + @@ -133,9 +39,9 @@ - - - + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 528038c5158b2..0d517ced97d17 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -14,8 +14,13 @@ - + + + + + + @@ -185,7 +190,7 @@ - + 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 a297697234ef2..602bd25ad8d27 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 @@ -15,6 +15,7 @@ + @@ -58,6 +59,11 @@ value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '")" if="$(var launch_start_planner_module)" /> + + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 02f14a86cd7df..1b09420f5047d 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -78,65 +78,33 @@ The parameters are set in `launch/ekf_localizer.launch` . ### For Node -| Name | Type | Description | Default value | -| :------------------------- | :----- | :---------------------------------------------------------------------------------------- | :------------ | -| show_debug_info | bool | Flag to display debug info | false | -| predict_frequency | double | Frequency for filtering and publishing [Hz] | 50.0 | -| tf_rate | double | Frequency for tf broadcasting [Hz] | 10.0 | -| publish_tf | bool | Whether to publish tf | true | -| extend_state_step | int | Max delay step which can be dealt with in EKF. Large number increases computational cost. | 50 | -| enable_yaw_bias_estimation | bool | Flag to enable yaw bias estimation | true | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/node.sub_schema.json") }} ### For pose measurement -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | -| pose_additional_delay | double | Additional delay time for pose measurement [s] | 0.0 | -| pose_measure_uncertainty_time | double | Measured time uncertainty used for covariance calculation [s] | 0.01 | -| pose_smoothing_steps | int | A value for smoothing steps | 5 | -| pose_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }} ### For twist measurement -| Name | Type | Description | Default value | -| :--------------------- | :----- | :-------------------------------------------------------- | :------------ | -| twist_additional_delay | double | Additional delay time for twist [s] | 0.0 | -| twist_smoothing_steps | int | A value for smoothing steps | 2 | -| twist_gate_dist | double | Limit of Mahalanobis distance used for outliers detection | 10000.0 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }} ### For process noise -| Name | Type | Description | Default value | -| :--------------------- | :----- | :--------------------------------------------------------------------------------------------------------------- | :------------ | -| proc_stddev_vx_c | double | Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0 | 2.0 | -| proc_stddev_wz_c | double | Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0 | 0.2 | -| proc_stddev_yaw_c | double | Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega | 0.005 | -| proc_stddev_yaw_bias_c | double | Standard deviation of process noise in time differentiation expression of yaw_bias, noise for d_yaw_bias = 0 | 0.001 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/process_noise.sub_schema.json") }} note: process noise for positions x & y are calculated automatically from nonlinear dynamics. ### Simple 1D Filter Parameters -| Name | Type | Description | Default value | -| :-------------------- | :----- | :---------------------------------------------- | :------------ | -| z_filter_proc_dev | double | Simple1DFilter - Z filter process deviation | 1.0 | -| roll_filter_proc_dev | double | Simple1DFilter - Roll filter process deviation | 0.01 | -| pitch_filter_proc_dev | double | Simple1DFilter - Pitch filter process deviation | 0.01 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }} ### For diagnostics -| Name | Type | Description | Default value | -| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| pose_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 50 | -| pose_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 250 | -| twist_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 50 | -| twist_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 250 | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json") }} ### Misc -| Name | Type | Description | Default value | -| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- | -| threshold_observable_velocity_mps | double | Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor | 0.0 (disabled) | +{{ json_to_markdown("localization/ekf_localizer/schema/sub/misc.sub_schema.json") }} ## How to tune EKF parameters diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 9de7f56f5c006..4cc71e0904ca8 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,38 +1,46 @@ /**: ros__parameters: - show_debug_info: false - enable_yaw_bias_estimation: true - predict_frequency: 50.0 - tf_rate: 50.0 - publish_tf: true - extend_state_step: 50 + node: + show_debug_info: false + enable_yaw_bias_estimation: true + predict_frequency: 50.0 + tf_rate: 50.0 + publish_tf: true + extend_state_step: 50 - # for Pose measurement - pose_additional_delay: 0.0 - pose_measure_uncertainty_time: 0.01 - pose_smoothing_steps: 5 - pose_gate_dist: 10000.0 + pose_measurement: + # for Pose measurement + pose_additional_delay: 0.0 + pose_measure_uncertainty_time: 0.01 + pose_smoothing_steps: 5 + pose_gate_dist: 10000.0 - # for twist measurement - twist_additional_delay: 0.0 - twist_smoothing_steps: 2 - twist_gate_dist: 10000.0 + twist_measurement: + # for twist measurement + twist_additional_delay: 0.0 + twist_smoothing_steps: 2 + twist_gate_dist: 10000.0 - # for process model - proc_stddev_yaw_c: 0.005 - proc_stddev_vx_c: 10.0 - proc_stddev_wz_c: 5.0 + process_noise: + # for process model + proc_stddev_yaw_c: 0.005 + proc_stddev_vx_c: 10.0 + proc_stddev_wz_c: 5.0 - #Simple1DFilter parameters - z_filter_proc_dev: 1.0 - roll_filter_proc_dev: 0.01 - pitch_filter_proc_dev: 0.01 + simple_1d_filter_parameters: + #Simple1DFilter parameters + z_filter_proc_dev: 1.0 + roll_filter_proc_dev: 0.01 + pitch_filter_proc_dev: 0.01 - # for diagnostics - pose_no_update_count_threshold_warn: 50 - pose_no_update_count_threshold_error: 100 - twist_no_update_count_threshold_warn: 50 - twist_no_update_count_threshold_error: 100 + diagnostics: + # for diagnostics + pose_no_update_count_threshold_warn: 50 + pose_no_update_count_threshold_error: 100 + twist_no_update_count_threshold_warn: 50 + twist_no_update_count_threshold_error: 100 - # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] + misc: + # for velocity measurement limitation (Set 0.0 if you want to ignore) + threshold_observable_velocity_mps: 0.0 # [m/s] + pose_frame_id: "map" diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 5139f900a339e..56a13d1ecaf55 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -24,36 +24,41 @@ class HyperParameters { public: explicit HyperParameters(rclcpp::Node * node) - : show_debug_info(node->declare_parameter("show_debug_info", false)), - ekf_rate(node->declare_parameter("predict_frequency", 50.0)), + : show_debug_info(node->declare_parameter("node.show_debug_info")), + ekf_rate(node->declare_parameter("node.predict_frequency")), ekf_dt(1.0 / std::max(ekf_rate, 0.1)), - tf_rate_(node->declare_parameter("tf_rate", 10.0)), - publish_tf_(node->declare_parameter("publish_tf", true)), - enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)), - extend_state_step(node->declare_parameter("extend_state_step", 50)), - pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))), - pose_additional_delay(node->declare_parameter("pose_additional_delay", 0.0)), - pose_gate_dist(node->declare_parameter("pose_gate_dist", 10000.0)), - pose_smoothing_steps(node->declare_parameter("pose_smoothing_steps", 5)), - twist_additional_delay(node->declare_parameter("twist_additional_delay", 0.0)), - twist_gate_dist(node->declare_parameter("twist_gate_dist", 10000.0)), - twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)), - proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)), - proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)), - proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)), - z_filter_proc_dev(node->declare_parameter("z_filter_proc_dev", 1.0)), - roll_filter_proc_dev(node->declare_parameter("roll_filter_proc_dev", 0.01)), - pitch_filter_proc_dev(node->declare_parameter("pitch_filter_proc_dev", 0.01)), + tf_rate_(node->declare_parameter("node.tf_rate")), + publish_tf_(node->declare_parameter("node.publish_tf")), + enable_yaw_bias_estimation(node->declare_parameter("node.enable_yaw_bias_estimation")), + extend_state_step(node->declare_parameter("node.extend_state_step")), + pose_frame_id(node->declare_parameter("misc.pose_frame_id")), + pose_additional_delay( + node->declare_parameter("pose_measurement.pose_additional_delay")), + pose_gate_dist(node->declare_parameter("pose_measurement.pose_gate_dist")), + pose_smoothing_steps(node->declare_parameter("pose_measurement.pose_smoothing_steps")), + twist_additional_delay( + node->declare_parameter("twist_measurement.twist_additional_delay")), + twist_gate_dist(node->declare_parameter("twist_measurement.twist_gate_dist")), + twist_smoothing_steps(node->declare_parameter("twist_measurement.twist_smoothing_steps")), + proc_stddev_vx_c(node->declare_parameter("process_noise.proc_stddev_vx_c")), + proc_stddev_wz_c(node->declare_parameter("process_noise.proc_stddev_wz_c")), + proc_stddev_yaw_c(node->declare_parameter("process_noise.proc_stddev_yaw_c")), + z_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.z_filter_proc_dev")), + roll_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.roll_filter_proc_dev")), + pitch_filter_proc_dev( + node->declare_parameter("simple_1d_filter_parameters.pitch_filter_proc_dev")), pose_no_update_count_threshold_warn( - node->declare_parameter("pose_no_update_count_threshold_warn", 50)), + node->declare_parameter("diagnostics.pose_no_update_count_threshold_warn")), pose_no_update_count_threshold_error( - node->declare_parameter("pose_no_update_count_threshold_error", 250)), + node->declare_parameter("diagnostics.pose_no_update_count_threshold_error")), twist_no_update_count_threshold_warn( - node->declare_parameter("twist_no_update_count_threshold_warn", 50)), + node->declare_parameter("diagnostics.twist_no_update_count_threshold_warn")), twist_no_update_count_threshold_error( - node->declare_parameter("twist_no_update_count_threshold_error", 250)), + node->declare_parameter("diagnostics.twist_no_update_count_threshold_error")), threshold_observable_velocity_mps( - node->declare_parameter("threshold_observable_velocity_mps", 0.5)) + node->declare_parameter("misc.threshold_observable_velocity_mps")) { } diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index ee0504293602a..b6a1bd06185c2 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -25,8 +25,6 @@ - - diff --git a/localization/ekf_localizer/schema/ekf_localizer.schema.json b/localization/ekf_localizer/schema/ekf_localizer.schema.json new file mode 100644 index 0000000000000..61fbcc2973aae --- /dev/null +++ b/localization/ekf_localizer/schema/ekf_localizer.schema.json @@ -0,0 +1,52 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration", + "type": "object", + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "node": { + "$ref": "sub/node.sub_schema.json#/definitions/node" + }, + "pose_measurement": { + "$ref": "sub/pose_measurement.sub_schema.json#/definitions/pose_measurement" + }, + "twist_measurement": { + "$ref": "sub/twist_measurement.sub_schema.json#/definitions/twist_measurement" + }, + "process_noise": { + "$ref": "sub/process_noise.sub_schema.json#/definitions/process_noise" + }, + "simple_1d_filter_parameters": { + "$ref": "sub/simple_1d_filter_parameters.sub_schema.json#/definitions/simple_1d_filter_parameters" + }, + "diagnostics": { + "$ref": "sub/diagnostics.sub_schema.json#/definitions/diagnostics" + }, + "misc": { + "$ref": "sub/misc.sub_schema.json#/definitions/misc" + } + }, + "required": [ + "node", + "pose_measurement", + "twist_measurement", + "process_noise", + "simple_1d_filter_parameters", + "diagnostics", + "misc" + ], + "additionalProperties": false + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json new file mode 100644 index 0000000000000..2e2dca411971e --- /dev/null +++ b/localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Diagnostics", + "definitions": { + "diagnostics": { + "type": "object", + "properties": { + "pose_no_update_count_threshold_warn": { + "type": "integer", + "description": "The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times", + "default": 50 + }, + "pose_no_update_count_threshold_error": { + "type": "integer", + "description": "The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times", + "default": 100 + }, + "twist_no_update_count_threshold_warn": { + "type": "integer", + "description": "The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times", + "default": 50 + }, + "twist_no_update_count_threshold_error": { + "type": "integer", + "description": "The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times", + "default": 100 + } + }, + "required": [ + "pose_no_update_count_threshold_warn", + "pose_no_update_count_threshold_error", + "twist_no_update_count_threshold_warn", + "twist_no_update_count_threshold_error" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/misc.sub_schema.json b/localization/ekf_localizer/schema/sub/misc.sub_schema.json new file mode 100644 index 0000000000000..cc36a5bf41ec6 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/misc.sub_schema.json @@ -0,0 +1,23 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for MISC", + "definitions": { + "misc": { + "type": "object", + "properties": { + "threshold_observable_velocity_mps": { + "type": "number", + "description": "Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor [m/s] (0.0 means disabled)", + "default": 0.0 + }, + "pose_frame_id": { + "type": "string", + "description": "Parent frame_id of EKF output pose", + "default": "map" + } + }, + "required": ["threshold_observable_velocity_mps", "pose_frame_id"], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/node.sub_schema.json b/localization/ekf_localizer/schema/sub/node.sub_schema.json new file mode 100644 index 0000000000000..92e083b92d3e7 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/node.sub_schema.json @@ -0,0 +1,50 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for node", + "definitions": { + "node": { + "type": "object", + "properties": { + "show_debug_info": { + "type": "boolean", + "description": "Flag to display debug info", + "default": false + }, + "predict_frequency": { + "type": "number", + "description": "Frequency for filtering and publishing [Hz]", + "default": 50.0 + }, + "tf_rate": { + "type": "number", + "description": "Frequency for tf broadcasting [Hz]", + "default": 50.0 + }, + "publish_tf": { + "type": "boolean", + "description": "Whether to publish tf", + "default": true + }, + "extend_state_step": { + "type": "integer", + "description": "Max delay step which can be dealt with in EKF. Large number increases computational cost.", + "default": 50 + }, + "enable_yaw_bias_estimation": { + "type": "boolean", + "description": "Flag to enable yaw bias estimation", + "default": true + } + }, + "required": [ + "show_debug_info", + "predict_frequency", + "tf_rate", + "publish_tf", + "extend_state_step", + "enable_yaw_bias_estimation" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json new file mode 100644 index 0000000000000..8b931d5f68d12 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Pose Measurement", + "definitions": { + "pose_measurement": { + "type": "object", + "properties": { + "pose_additional_delay": { + "type": "number", + "description": "Additional delay time for pose measurement [s]", + "default": 0.0 + }, + "pose_measure_uncertainty_time": { + "type": "number", + "description": "Measured time uncertainty used for covariance calculation [s]", + "default": 0.01 + }, + "pose_smoothing_steps": { + "type": "integer", + "description": "A value for smoothing steps", + "default": 5 + }, + "pose_gate_dist": { + "type": "number", + "description": "Limit of Mahalanobis distance used for outliers detection", + "default": 10000.0 + } + }, + "required": [ + "pose_additional_delay", + "pose_measure_uncertainty_time", + "pose_smoothing_steps", + "pose_gate_dist" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json new file mode 100644 index 0000000000000..37a8c248edd2c --- /dev/null +++ b/localization/ekf_localizer/schema/sub/process_noise.sub_schema.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Process Noise", + "definitions": { + "process_noise": { + "type": "object", + "properties": { + "proc_stddev_vx_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0", + "default": 10.0 + }, + "proc_stddev_wz_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0", + "default": 5.0 + }, + "proc_stddev_yaw_c": { + "type": "number", + "description": "Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega", + "default": 0.005 + } + }, + "required": ["proc_stddev_yaw_c", "proc_stddev_vx_c", "proc_stddev_wz_c"], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json new file mode 100644 index 0000000000000..ad2f638a18d5f --- /dev/null +++ b/localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration of Simple 1D Filter Parameters", + "definitions": { + "simple_1d_filter_parameters": { + "type": "object", + "properties": { + "z_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Z filter process deviation", + "default": 1.0 + }, + "roll_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Roll filter process deviation", + "default": 0.01 + }, + "pitch_filter_proc_dev": { + "type": "number", + "description": "Simple1DFilter - Pitch filter process deviation", + "default": 0.01 + } + }, + "required": ["z_filter_proc_dev", "roll_filter_proc_dev", "pitch_filter_proc_dev"], + "additionalProperties": false + } + } +} diff --git a/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json new file mode 100644 index 0000000000000..727830a90a288 --- /dev/null +++ b/localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "EKF Localizer Configuration for Twist Measurement", + "definitions": { + "twist_measurement": { + "type": "object", + "properties": { + "twist_additional_delay": { + "type": "number", + "description": "Additional delay time for twist [s]", + "default": 0.0 + }, + "twist_smoothing_steps": { + "type": "integer", + "description": "A value for smoothing steps", + "default": 2 + }, + "twist_gate_dist": { + "type": "number", + "description": "Limit of Mahalanobis distance used for outliers detection", + "default": 10000.0 + } + }, + "required": ["twist_additional_delay", "twist_smoothing_steps", "twist_gate_dist"], + "additionalProperties": false + } + } +} diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index f0c54d6393f7e..1a776bd810fff 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -2,7 +2,7 @@ **ArTagBasedLocalizer** is a vision-based localization node. - +ar_tag_image This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. @@ -30,6 +30,10 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | +## Parameters + +{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} + ## How to launch When launching Autoware, set `artag` for `pose_source`. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json new file mode 100644 index 0000000000000..bde5221aa1bbc --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json @@ -0,0 +1,87 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ar_tag_based_localizer", + "type": "object", + "definitions": { + "ar_tag_based_localizer": { + "type": "object", + "properties": { + "marker_size": { + "type": "number", + "description": "marker_size", + "default": 0.6 + }, + "target_tag_ids": { + "type": "array", + "description": "target_tag_ids", + "default": "['0','1','2','3','4','5','6']" + }, + "base_covariance": { + "type": "array", + "description": "base_covariance", + "default": [ + 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.02 + ] + }, + "distance_threshold": { + "type": "number", + "description": "distance_threshold(m)", + "default": "13.0" + }, + "consider_orientation": { + "type": "boolean", + "description": "consider_orientation", + "default": "false" + }, + "detection_mode": { + "type": "string", + "description": "detection_mode select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST]", + "default": "DM_NORMAL" + }, + "min_marker_size": { + "type": "number", + "description": "min_marker_size", + "default": 0.02 + }, + "ekf_time_tolerance": { + "type": "number", + "description": "ekf_time_tolerance(sec)", + "default": 5.0 + }, + "ekf_position_tolerance": { + "type": "number", + "description": "ekf_position_tolerance(m)", + "default": 10.0 + } + }, + "required": [ + "marker_size", + "target_tag_ids", + "base_covariance", + "distance_threshold", + "consider_orientation", + "detection_mode", + "min_marker_size", + "ekf_time_tolerance", + "ekf_position_tolerance" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ar_tag_based_localizer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md index 2ddf41e4eac70..7912ad843ef19 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -29,10 +29,4 @@ The package monitors the following two values: ## Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | -------------------------------------------------------------------------------------------- | -| `scale` | double | scale factor for monitored values (default: 3.0) | -| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | -| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | -| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | -| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.25) | +{{ json_to_markdown("localization/localization_error_monitor/schema/localization_error_monitor.schema.json") }} diff --git a/localization/localization_error_monitor/schema/localization_error_monitor.schema.json b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json new file mode 100644 index 0000000000000..0cdd5fb946756 --- /dev/null +++ b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Localization Error Monitor node", + "type": "object", + "definitions": { + "localization_error_monitor": { + "type": "object", + "properties": { + "scale": { + "type": "number", + "default": 3.0, + "description": "scale factor for monitored values" + }, + "error_ellipse_size": { + "type": "number", + "default": 1.5, + "description": "error threshold for long radius of confidence ellipse [m]" + }, + "warn_ellipse_size": { + "type": "number", + "default": 1.2, + "description": "warning threshold for long radius of confidence ellipse [m]" + }, + "error_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.3, + "description": "error threshold for size of confidence ellipse along lateral direction [m]" + }, + "warn_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.25, + "description": "warning threshold for size of confidence ellipse along lateral direction [m]" + } + }, + "required": [ + "scale", + "error_ellipse_size", + "warn_ellipse_size", + "error_ellipse_size_lateral_direction", + "warn_ellipse_size_lateral_direction" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/localization_error_monitor" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 0c5a4a7800639..375fccfa06787 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -23,7 +23,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy"; + key_value.key = "localization_error_ellipse"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); @@ -47,7 +47,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy_lateral_direction"; + key_value.key = "localization_error_ellipse_lateral_direction"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 95a7cebdc01c8..74d49e13a4c21 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -31,13 +31,13 @@ One optional function is regularization. Please see the regularization chapter i | `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | | `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] de-grounded pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | | `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | | `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | | `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | | `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | +| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | | `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | @@ -56,28 +56,29 @@ One optional function is regularization. Please see the regularization chapter i ### Core Parameters -| Name | Type | Description | -| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- | -| `base_frame` | string | Vehicle reference frame | -| `ndt_base_frame` | string | NDT reference frame | -| `map_frame` | string | map frame | -| `input_sensor_points_queue_size` | int | Subscriber queue size | -| `trans_epsilon` | double | The max difference between two consecutive transformations to consider convergence | -| `step_size` | double | The newton line search maximum step length | -| `resolution` | double | The ND voxel grid resolution [m] | -| `max_iterations` | int | The number of iterations required to calculate alignment | -| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | -| `converged_param_transform_probability` | double | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | -| `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | -| `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | -| `n_startup_trials` | int | The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). | -| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | -| `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | -| `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | -| `num_threads` | int | Number of threads used for parallel computing | -| `output_pose_covariance` | std::array | The covariance of output pose | - -(TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) +#### Frame + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }} + +#### Ndt + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }} + +#### Initial Pose Estimation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json") }} + +#### Validation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/validation.json") }} + +#### Score Estimation + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation.json") }} + +#### Covariance + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance.json") }} ## Regularization @@ -153,10 +154,7 @@ This is because if the base position is far off from the true value, NDT scan ma ### Parameters -| Name | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------- | -| `regularization_enabled` | bool | Flag to add regularization term to NDT optimization (FALSE by default) | -| `regularization_scale_factor` | double | Coefficient of the regularization term. | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt_regularization.json") }} Regularization is disabled by default because GNSS is not always accurate enough to serve the appropriate base position in any scenes. @@ -206,11 +204,7 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | ------------------------------------------------------------ | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json") }} ### Notes for dynamic map loading @@ -228,21 +222,16 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample | single file | at once (standard) | | multiple files | dynamically | -## Scan matching score based on de-grounded LiDAR scan +## Scan matching score based on no ground LiDAR scan ### Abstract -This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. +This is a function that uses no ground LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters - - -| Name | Type | Description | -| ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | -| `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | -| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json") }} ## 2D real-time covariance estimation @@ -261,8 +250,4 @@ Note that this function may spoil healthy system behavior if it consumes much ca initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. -| Name | Type | Description | -| ----------------------------- | ------------------- | ----------------------------------------------------------------- | -| `use_covariance_estimation` | bool | Flag for using real-time covariance estimation (FALSE by default) | -| `initial_pose_offset_model_x` | std::vector | X-axis offset [m] | -| `initial_pose_offset_model_y` | std::vector | Y-axis offset [m] | +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json") }} diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 9bc62d3f919c6..144449ce75084 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,103 +1,114 @@ /**: ros__parameters: - # Vehicle reference frame - base_frame: "base_link" + frame: + # Vehicle reference frame + base_frame: "base_link" - # NDT reference frame - ndt_base_frame: "ndt_base_link" + # NDT reference frame + ndt_base_frame: "ndt_base_link" - # map frame - map_frame: "map" + # Map frame + map_frame: "map" - # Subscriber queue size - input_sensor_points_queue_size: 1 - # The maximum difference between two consecutive - # transformations in order to consider convergence - trans_epsilon: 0.01 + ndt: + # The maximum difference between two consecutive + # transformations in order to consider convergence + trans_epsilon: 0.01 - # The newton line search maximum step length - step_size: 0.1 + # The newton line search maximum step length + step_size: 0.1 - # The ND voxel grid resolution - resolution: 2.0 + # The ND voxel grid resolution + resolution: 2.0 - # The number of iterations required to calculate alignment - max_iterations: 30 + # The number of iterations required to calculate alignment + max_iterations: 30 - # Converged param type - # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - converged_param_type: 1 + # Number of threads used for parallel computing + num_threads: 4 - # If converged_param_type is 0 - # Threshold for deciding whether to trust the estimation result - converged_param_transform_probability: 3.0 + regularization: + enable: false - # If converged_param_type is 1 - # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 + # Regularization scale factor + scale_factor: 0.01 - # The number of particles to estimate initial pose - initial_estimate_particles_num: 200 - # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). - # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. - # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. - n_startup_trials: 20 + initial_pose_estimation: + # The number of particles to estimate initial pose + particles_num: 200 - # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] - lidar_topic_timeout_sec: 1.0 + # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). + # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. + # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + n_startup_trials: 20 - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] - initial_pose_timeout_sec: 1.0 - # Tolerance of distance difference between two initial poses used for linear interpolation. [m] - initial_pose_distance_tolerance_m: 10.0 + validation: + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 - # Number of threads used for parallel computing - num_threads: 4 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 - # The covariance of output pose - # Note that this covariance matrix is empirically derived - output_pose_covariance: - [ - 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, - ] + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 - # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) - use_covariance_estimation: false + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100.0 - # Offset arrangement in covariance estimation [m] - # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. - initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] - initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] - # Regularization switch - regularization_enabled: false + score_estimation: + # Converged param type + # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD + converged_param_type: 1 - # Regularization scale factor - regularization_scale_factor: 0.01 + # If converged_param_type is 0 + # Threshold for deciding whether to trust the estimation result + converged_param_transform_probability: 3.0 - # Dynamic map loading distance - dynamic_map_loading_update_distance: 20.0 + # If converged_param_type is 1 + # Threshold for deciding whether to trust the estimation result + converged_param_nearest_voxel_transformation_likelihood: 2.3 - # Dynamic map loading loading radius - dynamic_map_loading_map_radius: 150.0 + # Scan matching score based on no ground LiDAR scan + no_ground_points: + enable: false - # Radius of input LiDAR range (used for diagnostics of dynamic map loading) - lidar_radius: 100.0 + # If lidar_point.z - base_link.z <= this threshold , the point will be removed + z_margin_for_ground_removal: 0.8 - # cspell: ignore degrounded - # A flag for using scan matching score based on de-grounded LiDAR scan - estimate_scores_for_degrounded_scan: false - # If lidar_point.z - base_link.z <= this threshold , the point will be removed - z_margin_for_ground_removal: 0.8 + covariance: + # The covariance of output pose + # Note that this covariance matrix is empirically derived + output_pose_covariance: + [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, + ] - # The execution time which means probably NDT cannot matches scans properly. [ms] - critical_upper_bound_exe_time_ms: 100 + # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) + covariance_estimation: + enable: false + + # Offset arrangement in covariance estimation [m] + # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. + initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + + + dynamic_map_loading: + # Dynamic map loading distance + update_distance: 20.0 + + # Dynamic map loading loading radius + map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 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 new file mode 100644 index 0000000000000..2955e3cb9a5f7 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -0,0 +1,173 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use node 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 NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ +#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ + +#include + +#include + +#include +#include +#include + +enum class ConvergedParamType { + TRANSFORM_PROBABILITY = 0, + NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 +}; + +struct HyperParameters +{ + struct Frame + { + std::string base_frame; + std::string ndt_base_frame; + std::string map_frame; + } frame; + + pclomp::NdtParams ndt; + bool ndt_regularization_enable; + + struct InitialPoseEstimation + { + int64_t particles_num; + int64_t n_startup_trials; + } initial_pose_estimation; + + struct Validation + { + double lidar_topic_timeout_sec; + double initial_pose_timeout_sec; + double initial_pose_distance_tolerance_m; + double critical_upper_bound_exe_time_ms; + } validation; + + struct ScoreEstimation + { + ConvergedParamType converged_param_type; + double converged_param_transform_probability; + double converged_param_nearest_voxel_transformation_likelihood; + struct NoGroundPoints + { + bool enable; + double z_margin_for_ground_removal; + } no_ground_points; + } score_estimation; + + struct Covariance + { + std::array output_pose_covariance; + + struct CovarianceEstimation + { + bool enable; + std::vector initial_pose_offset_model; + } covariance_estimation; + } covariance; + + struct DynamicMapLoading + { + double update_distance; + double map_radius; + double lidar_radius; + } dynamic_map_loading; + +public: + explicit HyperParameters(rclcpp::Node * node) + { + frame.base_frame = node->declare_parameter("frame.base_frame"); + frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); + frame.map_frame = node->declare_parameter("frame.map_frame"); + + ndt.trans_epsilon = node->declare_parameter("ndt.trans_epsilon"); + ndt.step_size = node->declare_parameter("ndt.step_size"); + ndt.resolution = node->declare_parameter("ndt.resolution"); + ndt.max_iterations = static_cast(node->declare_parameter("ndt.max_iterations")); + ndt.num_threads = static_cast(node->declare_parameter("ndt.num_threads")); + ndt.num_threads = std::max(ndt.num_threads, 1); + ndt_regularization_enable = node->declare_parameter("ndt.regularization.enable"); + ndt.regularization_scale_factor = + static_cast(node->declare_parameter("ndt.regularization.scale_factor")); + + initial_pose_estimation.particles_num = + node->declare_parameter("initial_pose_estimation.particles_num"); + initial_pose_estimation.n_startup_trials = + node->declare_parameter("initial_pose_estimation.n_startup_trials"); + + validation.lidar_topic_timeout_sec = + node->declare_parameter("validation.lidar_topic_timeout_sec"); + validation.initial_pose_timeout_sec = + node->declare_parameter("validation.initial_pose_timeout_sec"); + validation.initial_pose_distance_tolerance_m = + node->declare_parameter("validation.initial_pose_distance_tolerance_m"); + validation.critical_upper_bound_exe_time_ms = + node->declare_parameter("validation.critical_upper_bound_exe_time_ms"); + + const int64_t converged_param_type_tmp = + node->declare_parameter("score_estimation.converged_param_type"); + score_estimation.converged_param_type = + static_cast(converged_param_type_tmp); + score_estimation.converged_param_transform_probability = + node->declare_parameter("score_estimation.converged_param_transform_probability"); + score_estimation.converged_param_nearest_voxel_transformation_likelihood = + node->declare_parameter( + "score_estimation.converged_param_nearest_voxel_transformation_likelihood"); + score_estimation.no_ground_points.enable = + node->declare_parameter("score_estimation.no_ground_points.enable"); + score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter( + "score_estimation.no_ground_points.z_margin_for_ground_removal"); + + std::vector output_pose_covariance = + node->declare_parameter>("covariance.output_pose_covariance"); + for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { + covariance.output_pose_covariance[i] = output_pose_covariance[i]; + } + covariance.covariance_estimation.enable = + node->declare_parameter("covariance.covariance_estimation.enable"); + if (covariance.covariance_estimation.enable) { + std::vector initial_pose_offset_model_x = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_x"); + std::vector initial_pose_offset_model_y = + node->declare_parameter>( + "covariance.covariance_estimation.initial_pose_offset_model_y"); + + if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { + const size_t size = initial_pose_offset_model_x.size(); + covariance.covariance_estimation.initial_pose_offset_model.resize(size); + for (size_t i = 0; i < size; i++) { + covariance.covariance_estimation.initial_pose_offset_model[i].x() = + initial_pose_offset_model_x[i]; + covariance.covariance_estimation.initial_pose_offset_model[i].y() = + initial_pose_offset_model_y[i]; + } + } else { + RCLCPP_WARN( + node->get_logger(), + "Invalid initial pose offset model parameters. Disable covariance estimation."); + covariance.covariance_estimation.enable = false; + } + } + + dynamic_map_loading.update_distance = + node->declare_parameter("dynamic_map_loading.update_distance"); + dynamic_map_loading.map_radius = + node->declare_parameter("dynamic_map_loading.map_radius"); + dynamic_map_loading.lidar_radius = + node->declare_parameter("dynamic_map_loading.lidar_radius"); + } +}; + +#endif // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_ 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 8b192b0186b42..157421fc3ccb1 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 @@ -16,6 +16,7 @@ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #include "localization_util/util_func.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/particle.hpp" #include @@ -47,7 +48,8 @@ class MapUpdateModule public: MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr); + std::shared_ptr ndt_ptr, + HyperParameters::DynamicMapLoading param); private: friend class NDTScanMatcher; @@ -70,9 +72,8 @@ class MapUpdateModule rclcpp::Clock::SharedPtr clock_; std::optional last_update_position_ = std::nullopt; - const double dynamic_map_loading_update_distance_; - const double dynamic_map_loading_map_radius_; - const double lidar_radius_; + + HyperParameters::DynamicMapLoading param_; }; #endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 71895c59ec37d..ca69576061e21 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 @@ -18,6 +18,7 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" +#include "ndt_scan_matcher/hyper_parameters.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -65,11 +66,6 @@ #include #include -enum class ConvergedParamType { - TRANSFORM_PROBABILITY = 0, - NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 -}; - class NDTScanMatcher : public rclcpp::Node { using PointSource = pcl::PointXYZ; @@ -187,45 +183,21 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr> state_ptr_; Eigen::Matrix4f base_to_sensor_matrix_; - std::string base_frame_; - std::string ndt_base_frame_; - std::string map_frame_; - - ConvergedParamType converged_param_type_; - double converged_param_transform_probability_; - double converged_param_nearest_voxel_transformation_likelihood_; - - int64_t initial_estimate_particles_num_; - int64_t n_startup_trials_; - double lidar_topic_timeout_sec_; - double initial_pose_timeout_sec_; - double initial_pose_distance_tolerance_m_; - bool use_cov_estimation_; - std::vector initial_pose_offset_model_; - std::array output_pose_covariance_; std::mutex ndt_ptr_mtx_; std::unique_ptr initial_pose_buffer_; // Keep latest position for dynamic map loading - // This variable is only used when use_dynamic_map_loading is true std::mutex latest_ekf_position_mtx_; std::optional latest_ekf_position_ = std::nullopt; - // variables for regularization - const bool regularization_enabled_; // whether to use longitudinal regularization std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; - // cspell: ignore degrounded - bool estimate_scores_for_degrounded_scan_; - double z_margin_for_ground_removal_; - - // The execution time which means probably NDT cannot matches scans properly - int64_t critical_upper_bound_exe_time_ms_; + HyperParameters param_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json new file mode 100644 index 0000000000000..a42ee37858f46 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json @@ -0,0 +1,44 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "type": "object", + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "frame": { "$ref": "sub/frame.json#/definitions/frame" }, + "ndt": { "$ref": "sub/ndt.json#/definitions/ndt" }, + "regularization": { "$ref": "ndt_regularization.json#/definitions/ndt/regularization" }, + "initial_pose_estimation": { + "$ref": "sub/initial_pose_estimation.json#/definitions/initial_pose_estimation" + }, + "validation": { "$ref": "sub/validation.json#/definitions/validation" }, + "score_estimation": { + "$ref": "sub/score_estimation.json#/definitions/score_estimation" + }, + "covariance": { "$ref": "sub/covariance.json#/definitions/covariance" }, + "dynamic_map_loading": { + "$ref": "sub/dynamic_map_loading.json#/definitions/dynamic_map_loading" + } + }, + "required": [ + "frame", + "ndt", + "initial_pose_estimation", + "validation", + "score_estimation", + "covariance", + "dynamic_map_loading" + ], + "additionalProperties": false + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/ndt_scan_matcher/schema/sub/covariance.json b/localization/ndt_scan_matcher/schema/sub/covariance.json new file mode 100644 index 0000000000000..655edabbdf871 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/covariance.json @@ -0,0 +1,25 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "covariance": { + "type": "object", + "properties": { + "output_pose_covariance": { + "type": "array", + "description": "The covariance of output pose. Note that this covariance matrix is empirically derived.", + "default": [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0225, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625 + ] + }, + "covariance_estimation": { + "$ref": "covariance_covariance_estimation.json#/definitions/covariance_estimation" + } + }, + "required": ["output_pose_covariance", "covariance_estimation"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json new file mode 100644 index 0000000000000..d978e5bcf7357 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/covariance_covariance_estimation.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "covariance_estimation": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value).", + "default": false + }, + "initial_pose_offset_model_x": { + "type": "array", + "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", + "default": [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + }, + "initial_pose_offset_model_y": { + "type": "array", + "description": "Offset arrangement in covariance estimation [m]. initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.", + "default": [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + } + }, + "required": ["enable", "initial_pose_offset_model_x", "initial_pose_offset_model_y"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json b/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json new file mode 100644 index 0000000000000..9776fbed350f2 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/dynamic_map_loading.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "dynamic_map_loading": { + "type": "object", + "properties": { + "update_distance": { + "type": "number", + "description": "Dynamic map loading distance.", + "default": 20.0, + "minimum": 0.0 + }, + "map_radius": { + "type": "number", + "description": "Dynamic map loading loading radius.", + "default": 150.0, + "minimum": 0.0 + }, + "lidar_radius": { + "type": "number", + "description": "Radius of input LiDAR range (used for diagnostics of dynamic map loading).", + "default": 100.0, + "minimum": 0.0 + } + }, + "required": ["update_distance", "map_radius", "lidar_radius"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/frame.json b/localization/ndt_scan_matcher/schema/sub/frame.json new file mode 100644 index 0000000000000..98bf7abe711c3 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/frame.json @@ -0,0 +1,28 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "frame": { + "type": "object", + "properties": { + "base_frame": { + "type": "string", + "description": "Vehicle reference frame.", + "default": "base_link" + }, + "ndt_base_frame": { + "type": "string", + "description": "NDT reference frame.", + "default": "ndt_base_link" + }, + "map_frame": { + "type": "string", + "description": "Map frame.", + "default": "map" + } + }, + "required": ["base_frame", "ndt_base_frame", "map_frame"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json new file mode 100644 index 0000000000000..9817f3145bbd3 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json @@ -0,0 +1,25 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "initial_pose_estimation": { + "type": "object", + "properties": { + "particles_num": { + "type": "number", + "description": "The number of particles to estimate initial pose.", + "default": 200, + "minimum": 1 + }, + "n_startup_trials": { + "type": "number", + "description": "The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.", + "default": 20, + "minimum": 1 + } + }, + "required": ["particles_num", "n_startup_trials"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/ndt.json b/localization/ndt_scan_matcher/schema/sub/ndt.json new file mode 100644 index 0000000000000..850e48c2db33b --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/ndt.json @@ -0,0 +1,53 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "ndt": { + "type": "object", + "properties": { + "trans_epsilon": { + "type": "number", + "description": "The maximum difference between two consecutive transformations in order to consider convergence.", + "default": 0.01, + "minimum": 0.0 + }, + "step_size": { + "type": "number", + "description": "The newton line search maximum step length.", + "default": 0.1, + "minimum": 0.0 + }, + "resolution": { + "type": "number", + "description": "The ND voxel grid resolution.", + "default": 2.0, + "minimum": 0.0 + }, + "max_iterations": { + "type": "number", + "description": "The number of iterations required to calculate alignment.", + "default": 30, + "minimum": 1 + }, + "num_threads": { + "type": "number", + "description": "Number of threads used for parallel computing.", + "default": 4, + "minimum": 1 + }, + "regularization": { + "$ref": "ndt_regularization.json#/definitions/regularization" + } + }, + "required": [ + "trans_epsilon", + "step_size", + "resolution", + "max_iterations", + "num_threads", + "regularization" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json b/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json new file mode 100644 index 0000000000000..1de74de792700 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/ndt_regularization.json @@ -0,0 +1,24 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "regularization": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Regularization switch.", + "default": false + }, + "scale_factor": { + "type": "number", + "description": "Regularization scale factor.", + "default": 0.01, + "minimum": 0.0 + } + }, + "required": ["enable", "scale_factor"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation.json b/localization/ndt_scan_matcher/schema/sub/score_estimation.json new file mode 100644 index 0000000000000..c8a1195f99080 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/score_estimation.json @@ -0,0 +1,39 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "score_estimation": { + "type": "object", + "properties": { + "converged_param_type": { + "type": "number", + "description": "Converged param type. 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD", + "default": 1, + "minimum": 0, + "maximum": 1 + }, + "converged_param_transform_probability": { + "type": "number", + "description": "If converged_param_type is 0, threshold for deciding whether to trust the estimation result.", + "default": 3.0, + "minimum": 0.0 + }, + "converged_param_nearest_voxel_transformation_likelihood": { + "type": "number", + "description": "If converged_param_type is 1, threshold for deciding whether to trust the estimation result.", + "default": 2.3, + "minimum": 0.0 + }, + "no_ground_points": { + "$ref": "score_estimation_no_ground_points.json#/definitions/no_ground_points" + } + }, + "required": [ + "converged_param_type", + "converged_param_transform_probability", + "no_ground_points" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json b/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json new file mode 100644 index 0000000000000..aa3f0fe162bc0 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/score_estimation_no_ground_points.json @@ -0,0 +1,24 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "no_ground_points": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "A flag for using scan matching score based on de-grounded LiDAR scan.", + "default": false + }, + "z_margin_for_ground_removal": { + "type": "number", + "description": "If lidar_point.z - base_link.z <= this threshold , the point will be removed.", + "default": 0.8, + "minimum": 0.0 + } + }, + "required": ["enable", "z_margin_for_ground_removal"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/schema/sub/validation.json b/localization/ndt_scan_matcher/schema/sub/validation.json new file mode 100644 index 0000000000000..5ad40ceb99577 --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/validation.json @@ -0,0 +1,42 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "validation": { + "type": "object", + "properties": { + "lidar_topic_timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between current time and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "initial_pose_timeout_sec": { + "type": "number", + "description": "Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]", + "default": 1.0, + "minimum": 0.0 + }, + "initial_pose_distance_tolerance_m": { + "type": "number", + "description": "Tolerance of distance difference between two initial poses used for linear interpolation. [m]", + "default": 10.0, + "minimum": 0.0 + }, + "critical_upper_bound_exe_time_ms": { + "type": "number", + "description": "The execution time which means probably NDT cannot matches scans properly. [ms]", + "default": 100.0, + "minimum": 0.0 + } + }, + "required": [ + "lidar_topic_timeout_sec", + "initial_pose_timeout_sec", + "initial_pose_distance_tolerance_m", + "critical_upper_bound_exe_time_ms" + ], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 39b92fe248660..402ec9da32782 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -16,16 +16,12 @@ MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr) + std::shared_ptr ndt_ptr, HyperParameters::DynamicMapLoading param) : ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), logger_(node->get_logger()), clock_(node->get_clock()), - dynamic_map_loading_update_distance_( - node->declare_parameter("dynamic_map_loading_update_distance")), - dynamic_map_loading_map_radius_( - node->declare_parameter("dynamic_map_loading_map_radius")), - lidar_radius_(node->declare_parameter("lidar_radius")) + param_(param) { loaded_pcd_pub_ = node->create_publisher( "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); @@ -42,10 +38,10 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi const double dx = position.x - last_update_position_.value().x; const double dy = position.y - last_update_position_.value().y; const double distance = std::hypot(dx, dy); - if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { + if (distance + param_.lidar_radius > param_.map_radius) { RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); } - return distance > dynamic_map_loading_update_distance_; + return distance > param_.update_distance; } void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) @@ -53,7 +49,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) auto request = std::make_shared(); request->area.center_x = static_cast(position.x); request->area.center_y = static_cast(position.y); - request->area.radius = static_cast(dynamic_map_loading_map_radius_); + request->area.radius = static_cast(param_.map_radius); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { 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 942df03410165..cd637791f04b6 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -64,7 +64,6 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -// cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), @@ -72,99 +71,11 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - output_pose_covariance_({}), - regularization_enabled_(declare_parameter("regularization_enabled")), - is_activated_(false) + is_activated_(false), + param_(this) { (*state_ptr_)["state"] = "Initializing"; - int64_t points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); - points_queue_size = std::max(points_queue_size, (int64_t)0); - RCLCPP_INFO(get_logger(), "points_queue_size: %ld", points_queue_size); - - base_frame_ = this->declare_parameter("base_frame"); - RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); - - ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); - RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); - - map_frame_ = this->declare_parameter("map_frame"); - RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str()); - - pclomp::NdtParams ndt_params{}; - ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); - ndt_params.step_size = this->declare_parameter("step_size"); - ndt_params.resolution = this->declare_parameter("resolution"); - ndt_params.max_iterations = static_cast(this->declare_parameter("max_iterations")); - ndt_params.num_threads = static_cast(this->declare_parameter("num_threads")); - ndt_params.num_threads = std::max(ndt_params.num_threads, 1); - ndt_params.regularization_scale_factor = - static_cast(this->declare_parameter("regularization_scale_factor")); - ndt_ptr_->setParams(ndt_params); - - RCLCPP_INFO( - get_logger(), "trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", - ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution, - ndt_params.max_iterations); - - const int64_t converged_param_type_tmp = this->declare_parameter("converged_param_type"); - converged_param_type_ = static_cast(converged_param_type_tmp); - - converged_param_transform_probability_ = - this->declare_parameter("converged_param_transform_probability"); - converged_param_nearest_voxel_transformation_likelihood_ = - this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); - - lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); - - critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms"); - - initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); - - initial_pose_distance_tolerance_m_ = - this->declare_parameter("initial_pose_distance_tolerance_m"); - - initial_pose_buffer_ = std::make_unique( - this->get_logger(), initial_pose_timeout_sec_, initial_pose_distance_tolerance_m_); - - use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); - if (use_cov_estimation_) { - std::vector initial_pose_offset_model_x = - this->declare_parameter>("initial_pose_offset_model_x"); - std::vector initial_pose_offset_model_y = - this->declare_parameter>("initial_pose_offset_model_y"); - - if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { - const size_t size = initial_pose_offset_model_x.size(); - initial_pose_offset_model_.resize(size); - for (size_t i = 0; i < size; i++) { - initial_pose_offset_model_[i].x() = initial_pose_offset_model_x[i]; - initial_pose_offset_model_[i].y() = initial_pose_offset_model_y[i]; - } - } else { - RCLCPP_WARN( - get_logger(), - "Invalid initial pose offset model parameters. Disable covariance estimation."); - use_cov_estimation_ = false; - } - } - - std::vector output_pose_covariance = - this->declare_parameter>("output_pose_covariance"); - for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { - output_pose_covariance_[i] = output_pose_covariance[i]; - } - - initial_estimate_particles_num_ = - this->declare_parameter("initial_estimate_particles_num"); - n_startup_trials_ = this->declare_parameter("n_startup_trials"); - - estimate_scores_for_degrounded_scan_ = - this->declare_parameter("estimate_scores_for_degrounded_scan"); - - z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); - timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -187,12 +98,12 @@ NDTScanMatcher::NDTScanMatcher() std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); sensor_points_sub_ = this->create_subscription( - "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), + "points_raw", rclcpp::SensorDataQoS().keep_last(1), std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), sensor_sub_opt); // Only if regularization is enabled, subscribe to the regularization base pose - if (regularization_enabled_) { + if (param_.ndt_regularization_enable) { // NOTE: The reason that the regularization subscriber does not belong to the // sensor_callback_group is to ensure that the regularization callback is called even if // sensor_callback takes long time to process. @@ -264,7 +175,14 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); + ndt_ptr_->setParams(param_.ndt); + + initial_pose_buffer_ = std::make_unique( + this->get_logger(), param_.validation.initial_pose_timeout_sec, + param_.validation.initial_pose_distance_tolerance_m); + + map_update_module_ = + std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); logger_configure_ = std::make_unique(this); } @@ -290,7 +208,8 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("lidar_topic_delay_time_sec") && - std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > + param_.validation.lidar_topic_timeout_sec) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; } @@ -310,14 +229,13 @@ void NDTScanMatcher::publish_diagnostic() if ( state_ptr_->count("nearest_voxel_transformation_likelihood") && std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < - converged_param_nearest_voxel_transformation_likelihood_) { + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } if ( - state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= - static_cast(critical_upper_bound_exe_time_ms_)) { + state_ptr_->count("execution_time") && std::stod((*state_ptr_)["execution_time"]) >= + param_.validation.critical_upper_bound_exe_time_ms) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; @@ -362,13 +280,13 @@ void NDTScanMatcher::callback_initial_pose( { if (!is_activated_) return; - if (initial_pose_msg_ptr->header.frame_id == map_frame_) { + if (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame) { initial_pose_buffer_->push_back(initial_pose_msg_ptr); } else { RCLCPP_ERROR_STREAM_THROTTLE( get_logger(), *this->get_clock(), 1000, "Received initial pose message with frame_id " - << initial_pose_msg_ptr->header.frame_id << ", but expected " << map_frame_ + << initial_pose_msg_ptr->header.frame_id << ", but expected " << param_.frame.map_frame << ". Please check the frame_id in the input topic and ensure it is correct."); } @@ -397,12 +315,12 @@ void NDTScanMatcher::callback_sensor_points( const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); - if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + if (lidar_topic_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { RCLCPP_WARN( this->get_logger(), "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " "%lf[sec])", - lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + lidar_topic_delay_time_sec, param_.validation.lidar_topic_timeout_sec); // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, // even if further processing continues, the estimated result will be rejected by ekf_localizer. @@ -426,7 +344,8 @@ void NDTScanMatcher::callback_sensor_points( pcl::fromROSMsg(*sensor_points_msg_in_sensor_frame, *sensor_points_in_sensor_frame); transform_sensor_measurement( - sensor_frame, base_frame_, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); + sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, + sensor_points_in_baselink_frame); ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); if (!is_activated_) return; @@ -442,7 +361,7 @@ void NDTScanMatcher::callback_sensor_points( interpolation_result_opt.value(); // if regularization is enabled and available, set pose to NDT for regularization - if (regularization_enabled_) { + if (param_.ndt_regularization_enable) { add_regularization_pose(sensor_ros_time); } @@ -493,9 +412,9 @@ void NDTScanMatcher::callback_sensor_points( map_to_base_link_quat.normalized().toRotationMatrix(); std::array ndt_covariance = - rotate_covariance(output_pose_covariance_, map_to_base_link_rotation); + rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); - if (is_converged && use_cov_estimation_) { + if (is_converged && param_.covariance.covariance_estimation.enable) { const auto estimated_covariance = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); ndt_covariance = estimated_covariance; @@ -525,16 +444,18 @@ void NDTScanMatcher::callback_sensor_points( new pcl::PointCloud); tier4_autoware_utils::transformPointCloud( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud(sensor_ros_time, param_.frame.map_frame, sensor_points_in_map_ptr); - // whether use de-grounded points calculate score - if (estimate_scores_for_degrounded_scan_) { + // whether use no ground points to calculate score + if (param_.score_estimation.no_ground_points.enable) { // remove ground pcl::shared_ptr> no_ground_points_in_map_ptr( new pcl::PointCloud); for (std::size_t i = 0; i < sensor_points_in_map_ptr->size(); i++) { const float point_z = sensor_points_in_map_ptr->points[i].z; // NOLINT - if (point_z - matrix4f_to_pose(ndt_result.pose).position.z > z_margin_for_ground_removal_) { + if ( + point_z - matrix4f_to_pose(ndt_result.pose).position.z > + param_.score_estimation.no_ground_points.z_margin_for_ground_removal) { no_ground_points_in_map_ptr->points.push_back(sensor_points_in_map_ptr->points[i]); } } @@ -542,7 +463,7 @@ void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2 no_ground_points_msg_in_map; pcl::toROSMsg(*no_ground_points_in_map_ptr, no_ground_points_msg_in_map); no_ground_points_msg_in_map.header.stamp = sensor_ros_time; - no_ground_points_msg_in_map.header.frame_id = map_frame_; + no_ground_points_msg_in_map.header.frame_id = param_.frame.map_frame; no_ground_points_aligned_pose_pub_->publish(no_ground_points_msg_in_map); // calculate score const auto no_ground_transform_probability = static_cast( @@ -605,10 +526,10 @@ void NDTScanMatcher::publish_tf( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; tf2_broadcaster_.sendTransform( - tier4_autoware_utils::pose2transform(result_pose_stamped_msg, ndt_base_frame_)); + tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } void NDTScanMatcher::publish_pose( @@ -617,12 +538,12 @@ void NDTScanMatcher::publish_pose( { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; - result_pose_stamped_msg.header.frame_id = map_frame_; + result_pose_stamped_msg.header.frame_id = param_.frame.map_frame; result_pose_stamped_msg.pose = result_pose_msg; geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = sensor_ros_time; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = result_pose_msg; result_pose_with_cov_msg.pose.covariance = ndt_covariance; @@ -649,7 +570,7 @@ void NDTScanMatcher::publish_marker( visualization_msgs::msg::MarkerArray marker_array; visualization_msgs::msg::Marker marker; marker.header.stamp = sensor_ros_time; - marker.header.frame_id = map_frame_; + marker.header.frame_id = param_.frame.map_frame; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1); @@ -683,7 +604,7 @@ void NDTScanMatcher::publish_initial_to_result( initial_to_result_relative_pose_stamped.pose = tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; - initial_to_result_relative_pose_stamped.header.frame_id = map_frame_; + initial_to_result_relative_pose_stamped.header.frame_id = param_.frame.map_frame; initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); const auto initial_to_result_distance = @@ -731,13 +652,16 @@ bool NDTScanMatcher::validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood) { bool is_ok_converged_param = false; - if (converged_param_type_ == ConvergedParamType::TRANSFORM_PROBABILITY) { + if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { is_ok_converged_param = validate_score( - transform_probability, converged_param_transform_probability_, "Transform Probability"); - } else if (converged_param_type_ == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { + transform_probability, param_.score_estimation.converged_param_transform_probability, + "Transform Probability"); + } else if ( + param_.score_estimation.converged_param_type == + ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { is_ok_converged_param = validate_score( nearest_voxel_transformation_likelihood, - converged_param_nearest_voxel_transformation_likelihood_, + param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood, "Nearest Voxel Transformation Likelihood"); } else { is_ok_converged_param = false; @@ -806,11 +730,12 @@ std::array NDTScanMatcher::estimate_covariance( ndt_result.hessian.inverse().block(0, 0, 2, 2)); } catch (const std::exception & e) { RCLCPP_WARN(get_logger(), "Error in Eigen solver: %s", e.what()); - return output_pose_covariance_; + return param_.covariance.output_pose_covariance; } // first result is added to mean - const int n = static_cast(initial_pose_offset_model_.size()) + 1; + const int n = + static_cast(param_.covariance.covariance_estimation.initial_pose_offset_model.size()) + 1; const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); Eigen::Vector2d mean = ndt_pose_2d; std::vector ndt_pose_2d_vec; @@ -820,14 +745,15 @@ std::array NDTScanMatcher::estimate_covariance( 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; - multi_ndt_result_msg.header.frame_id = map_frame_; + multi_ndt_result_msg.header.frame_id = param_.frame.map_frame; multi_initial_pose_msg.header.stamp = sensor_ros_time; - multi_initial_pose_msg.header.frame_id = map_frame_; + multi_initial_pose_msg.header.frame_id = param_.frame.map_frame; multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); // multiple searches - for (const auto & pose_offset : initial_pose_offset_model_) { + for (const auto & pose_offset : + param_.covariance.covariance_estimation.initial_pose_offset_model) { const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); @@ -856,7 +782,7 @@ std::array NDTScanMatcher::estimate_covariance( } pca_covariance /= (n - 1); // unbiased covariance - std::array ndt_covariance = output_pose_covariance_; + std::array ndt_covariance = param_.covariance.output_pose_covariance; ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); @@ -900,7 +826,7 @@ void NDTScanMatcher::service_ndt_align( tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame - const std::string & target_frame = map_frame_; + const std::string & target_frame = param_.frame.map_frame; const std::string & source_frame = req->pose_with_covariance.header.frame_id; geometry_msgs::msg::TransformStamped transform_s2t; @@ -984,7 +910,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // the ego vehicle is aligned with the ground to some extent about roll and pitch. const std::vector is_loop_variable = {false, false, false, false, false, true}; TreeStructuredParzenEstimator tpe( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials_, is_loop_variable); + TreeStructuredParzenEstimator::Direction::MAXIMIZE, + param_.initial_pose_estimation.n_startup_trials, is_loop_variable); std::vector particle_array; auto output_cloud = std::make_shared>(); @@ -992,9 +919,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // publish the estimated poses in 20 times to see the progress and to avoid dropping data visualization_msgs::msg::MarkerArray marker_array; constexpr int64_t publish_num = 20; - const int64_t publish_interval = initial_estimate_particles_num_ / publish_num; + const int64_t publish_interval = param_.initial_pose_estimation.particles_num / publish_num; - for (int64_t i = 0; i < initial_estimate_particles_num_; i++) { + for (int64_t i = 0; i < param_.initial_pose_estimation.particles_num; i++) { const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); geometry_msgs::msg::Pose initial_pose; @@ -1020,8 +947,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, ndt_result.iteration_num); particle_array.push_back(particle); - push_debug_markers(marker_array, get_clock()->now(), map_frame_, particle, i); - if ((i + 1) % publish_interval == 0 || (i + 1) == initial_estimate_particles_num_) { + push_debug_markers(marker_array, get_clock()->now(), param_.frame.map_frame, particle, i); + if ( + (i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.particles_num) { ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); marker_array.markers.clear(); } @@ -1050,7 +978,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); - publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_in_map_ptr); + publish_point_cloud( + initial_pose_with_cov.header.stamp, param_.frame.map_frame, sensor_points_in_map_ptr); } auto best_particle_ptr = std::max_element( @@ -1059,7 +988,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; - result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); diff --git a/localization/pose_estimator_arbiter/CMakeLists.txt b/localization/pose_estimator_arbiter/CMakeLists.txt new file mode 100644 index 0000000000000..9a47b654a6ab4 --- /dev/null +++ b/localization/pose_estimator_arbiter/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.14) +project(pose_estimator_arbiter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(glog REQUIRED) + +find_package(PCL REQUIRED COMPONENTS common) +include_directories(SYSTEM ${PCL_INCLUDE_DIRS}) + +# ============================== +# switch rule library +ament_auto_add_library(switch_rule + SHARED + src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp +) +target_include_directories(switch_rule PUBLIC src) + +# ============================== +# pose estimator arbiter node +ament_auto_add_executable(${PROJECT_NAME} + src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp + src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC src) +target_link_libraries(${PROJECT_NAME} switch_rule glog::glog) + +# ============================== +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + # define test definition macro + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) + target_link_libraries(${test_name} fmt) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + add_testcase(test/test_shared_data.cpp) + add_ros_test( + test/test_pose_estimator_arbiter.py + TIMEOUT "30" + ) +endif() + +# ============================== +# In practice, the example rule is not used as autoware code. +# It exists only for user reference and is tested only. +add_subdirectory(example_rule) + +# ============================== +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md new file mode 100644 index 0000000000000..3a32d5883642e --- /dev/null +++ b/localization/pose_estimator_arbiter/README.md @@ -0,0 +1,284 @@ +# pose_estimator_arbiter + +Table of contents: + +- [Abstract](#abstract) +- [Interface](#interfaces) +- [Architecture](#architecture) +- [How to launch](#how-to-launch) +- [Switching Rules](#switching-rules) +- [Pose Initialization](#pose-initialization) +- [Future Plans](#future-plans) + +## Abstract + +This package launches multiple pose estimators and provides the capability to stop or resume specific pose estimators based on the situation. +It provides provisional switching rules and will be adaptable to a wide variety of rules in the future. + +Please refer to [this discussion](https://github.com/orgs/autowarefoundation/discussions/3878) about other ideas on implementation. + +### Why do we need a stop/resume mechanism? + +It is possible to launch multiple pose_estimators and fuse them using a Kalman filter by editing launch files. +However, this approach is not preferable due to computational costs. + +Particularly, NDT and YabLoc are computationally intensive, and it's not recommended to run them simultaneously. +Also, even if both can be activated at the same time, the Kalman Filter may be affected by one of them giving bad output. + +> [!NOTE] +> Currently, **there is ONLY A RULE implemented that always enables all pose_estimators.** +> If users want to toggle pose_estimator with their own rules, they need to add new rules. by referring to example_rule. +> The [example_rule](example_rule/README.md) has source code that can be used as a reference for implementing the rules. + +### Supporting pose_estimators + +- [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) +- [eagleye](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/launch-autoware/localization/eagleye/) +- [yabloc](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/yabloc) +- [landmark_based_localizer](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/landmark_based_localizer) + +### Demonstration + +The following video demonstrates the switching of four different pose estimators. + +
+ +Users can reproduce the demonstration using the following data and launch command: + +[sample data (rosbag & map)](https://drive.google.com/file/d/1MxLo1Sw6PdvfkyOYf_9A5dZ9uli1vPvS/view) +The rosbag is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +The map is an edited version of the [original map data](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip) published on the AWSIM documentation page to make it suitable for multiple pose_estimators. + +```bash +ros2 launch autoware_launch logging_simulator.launch.xml \ + map_path:= \ + vehicle_model:=sample_vehicle \ + sensor_model:=awsim_sensor_kit \ + pose_source:=ndt_yabloc_artag_eagleye +``` + +## Interfaces + +
+Click to show details + +### Parameters + +There are no parameters. + +### Services + +| Name | Type | Description | +| ---------------- | ------------------------------- | ------------------------------- | +| `/config_logger` | logging_demo::srv::ConfigLogger | service to modify logging level | + +### Clients + +| Name | Type | Description | +| --------------------- | --------------------- | --------------------------------- | +| `/yabloc_suspend_srv` | std_srv::srv::SetBool | service to stop or restart yabloc | + +### Subscriptions + +For pose estimator arbitration: + +| Name | Type | Description | +| ------------------------------------- | --------------------------------------------- | -------------- | +| `/input/artag/image` | sensor_msgs::msg::Image | ArTag input | +| `/input/yabloc/image` | sensor_msgs::msg::Image | YabLoc input | +| `/input/eagleye/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | Eagleye output | +| `/input/ndt/pointcloud` | sensor_msgs::msg::PointCloud2 | NDT input | + +For switching rule: + +| Name | Type | Description | +| ----------------------------- | ------------------------------------------------------------ | --------------------------------- | +| `/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `/input/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | localization final output | +| `/input/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | localization initialization state | + +### Publications + +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------- | ------------------------------------------------------ | +| `/output/artag/image` | sensor_msgs::msg::Image | relayed ArTag input | +| `/output/yabloc/image` | sensor_msgs::msg::Image | relayed YabLoc input | +| `/output/eagleye/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | relayed Eagleye output | +| `/output/ndt/pointcloud` | sensor_msgs::msg::PointCloud2 | relayed NDT input | +| `/output/debug/marker_array` | visualization_msgs::msg::MarkerArray | [debug topic] everything for visualization | +| `/output/debug/string` | visualization_msgs::msg::MarkerArray | [debug topic] debug information such as current status | + +
+ +## Trouble Shooting + +If it does not seems to work, users can get more information in the following ways. + +> [!TIP] +> +> ```bash +> ros2 service call /localization/pose_estimator_arbiter/config_logger logging_demo/srv/ConfigLogger \ +> '{logger_name: localization.pose_estimator_arbiter, level: debug}' +> ``` + +## Architecture + +
+Click to show details + +### Case of running a single pose estimator + +When each pose_estimator is run alone, this package does nothing. +Following figure shows the node configuration when NDT, YabLoc Eagleye and AR-Tag are run independently. + +drawing + +### Case of running multiple pose estimators + +When running multiple pose_estimators, pose_estimator_arbiter is executed. +It comprises a **switching rule** and **stoppers** corresponding to each pose_estimator. + +- Stoppers control the pose_estimator activity by relaying inputs or outputs, or by requesting a suspend service. +- Switching rules determine which pose_estimator to use. + +Which stoppers and switching rules are instantiated depends on the runtime arguments at startup. + +Following figure shows the node configuration when all pose_estimator are run simultaneously. + +drawing + +- **NDT** + +The NDT stopper relays topics in the front side of the point cloud pre-processor. + +- **YabLoc** + +The YabLoc stopper relays input image topics in the frontend of the image pre-processor. +YabLoc includes a particle filter process that operates on a timer, and even when image topics are not streamed, the particle prediction process continues to work. +To address this, the YabLoc stopper also has a service client for explicitly stopping and resuming YabLoc. + +- **Eagleye** + +The Eagleye stopper relays Eagleye's output pose topics in the backend of Eagleye's estimation process. +Eagleye performs time-series processing internally, and it can't afford to stop the input stream. +Furthermore, Eagleye's estimation process is lightweight enough to be run continuously without a significant load, so the relay is inserted in the backend. + +- **ArTag** + +The ArTag stopper relays image topics in the front side of the landmark localizer. + +
+ +## How to launch + +
+Click to show details + +The user can launch the desired pose_estimators by giving the pose_estimator names as a concatenation of underscores for the runtime argument `pose_source`. + +```bash +ros2 launch autoware_launch logging_simulator.launch.xml \ + map_path:= \ + vehicle_model:=sample_vehicle \ + sensor_model:=awsim_sensor_kit \ + pose_source:=ndt_yabloc_artag_eagleye +``` + +Even if `pose_source` includes an unexpected string, it will be filtered appropriately. +Please see the table below for details. + +| given runtime argument | parsed pose_estimator_arbiter's param (pose_sources) | +| ------------------------------------------- | ---------------------------------------------------- | +| `pose_source:=ndt` | `["ndt"]` | +| `pose_source:=nan` | `[]` | +| `pose_source:=yabloc_ndt` | `["ndt","yabloc"]` | +| `pose_source:=yabloc_ndt_ndt_ndt` | `["ndt","yabloc"]` | +| `pose_source:=ndt_yabloc_eagleye` | `["ndt","yabloc","eagleye"]` | +| `pose_source:=ndt_yabloc_nan_eagleye_artag` | `["ndt","yabloc","eagleye","artag"]` | + +
+ +## Switching Rules + +
+Click to show details + +Currently, **ONLY ONE RULE** (`enable_all_rule`) is implemented. +In the future, several rules will be implemented and users will be able to select rules. + +> [!TIP] +> There are presets available to extend the rules. If you want to extend the rules, please see [example_rule](./example_rule/README.md). + +### Enable All Rule + +This is the default and simplest rule. This rule enables all pose_estimators regardless of their current state. + +```mermaid +flowchart LR + A{ } + A --whatever --> _A[enable all pose_estimators] +``` + +
+ +## Pose Initialization + +When using multiple pose_estimators, it is necessary to appropriately adjust the parameters provided to the `pose_initializer`. + +
+Click to show details + +The following table is based on the runtime argument "pose_source" indicating which initial pose estimation methods are available and the parameters that should be provided to the pose_initialization node. +To avoid making the application too complicated, a priority is established so that NDT is always used when it is available. +(The pose_initializer will only perform NDT-based initial pose estimation when `ndt_enabled` and `yabloc_enabled` are both `true`). + +This table's usage is described from three perspectives: + +- **Autoware Users:** Autoware users do not need to consult this table. + They simply provide the desired combinations of pose_estimators, and the appropriate parameters are automatically provided to the pose_initializer. +- **Autoware Developers:** Autoware developers can consult this table to know which parameters are assigned. +- **Who implements New Pose Estimator Switching:** + Developers must extend this table and implement the assignment of appropriate parameters to the pose_initializer. + +| pose_source | invoked initialization method | `ndt_enabled` | `yabloc_enabled` | `gnss_enabled` | `sub_gnss_pose_cov` | +| :-------------------------: | ----------------------------- | ------------- | ---------------- | -------------- | -------------------------------------------- | +| ndt | ndt | true | false | true | /sensing/gnss/pose_with_covariance | +| yabloc | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| eagleye | vehicle needs run for a while | false | false | true | /localization/pose_estimator/eagleye/... | +| artag | 2D Pose Estimate (RViz) | false | false | true | /sensing/gnss/pose_with_covariance | +| ndt, yabloc | ndt | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| ndt, artag | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| yabloc, eagleye | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| yabloc, artag | yabloc | false | true | true | /sensing/gnss/pose_with_covariance | +| eagleye, artag | vehicle needs run for a while | false | false | true | /localization/pose_estimator/eagleye/pose... | +| ndt, yabloc, eagleye | ndt | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, eagleye, artag | ndt | ndt | false | true | /sensing/gnss/pose_with_covariance | +| yabloc, eagleye, artag | yabloc | ndt | true | true | /sensing/gnss/pose_with_covariance | +| ndt, yabloc, eagleye, artag | ndt | ndt | true | true | /sensing/gnss/pose_with_covariance | + +
+ +## Future Plans + +
+Click to show details + +### gradually switching + +In the future, this package will provide not only ON/OFF switching, but also a mechanism for low frequency operation, such as 50% NDT & 50% YabLoc. + +### stopper for pose_estimators to be added in the future + +The basic strategy is to realize ON/OFF switching by relaying the input or output topics of that pose_estimator. +If pose_estimator involves time-series processing with heavy computations, it's not possible to pause and resume with just topic relaying. + +In such cases, there may not be generally applicable solutions, but the following methods may help: + +1. Completely stop and **reinitialize** time-series processing, as seen in the case of YabLoc. +2. Subscribe to `localization/kinematic_state` and **keep updating states** to ensure that the estimation does not break (relying on the output of the active pose_estimator). +3. The multiple pose_estimator **does not support** that particular pose_estimator. + +Please note that this issue is fundamental to realizing multiple pose_estimators, and it will arise regardless of the architecture proposed in this case. + +
diff --git a/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt new file mode 100644 index 0000000000000..333f92842b860 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/CMakeLists.txt @@ -0,0 +1,31 @@ +# example switch rule library +ament_auto_add_library(example_rule + SHARED + src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp + src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp + src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp + src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp +) +target_include_directories(example_rule PUBLIC src example_rule/src) +target_link_libraries(example_rule switch_rule) + +# ============================== +# define test definition macro +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gtest(${test_name} ${filepath}) + target_link_libraries("${test_name}" switch_rule example_rule) + target_include_directories(${test_name} PUBLIC src) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +# ============================== +# build test +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + add_testcase(test/test_rule_helper.cpp) + add_testcase(test/test_vector_map_based_rule.cpp) + add_testcase(test/test_pcd_map_based_rule.cpp) +endif() diff --git a/localization/pose_estimator_arbiter/example_rule/README.md b/localization/pose_estimator_arbiter/example_rule/README.md new file mode 100644 index 0000000000000..c5bb80912607f --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/README.md @@ -0,0 +1,99 @@ +# example rule + +The example rule provides a sample rule for controlling the arbiter. By combining the provided rules, it is possible to achieve demonstrations as follows. Users can extend the rules as needed by referencing this code, allowing them to control the arbiter as desired. + +## Demonstration + +The following video demonstrates the switching of four different pose estimators. + +
+ +## Switching Rules + +### Pcd Map Based Rule + +```mermaid +flowchart LR + A{PCD is enough dense } + A --true--> B[enable NDT] + A --false--> C[enable YabLoc] +``` + +### Vector Map Based Rule + +```mermaid +flowchart LR + A{ } + A --whatever --> _A[When the ego vehicle is in a predetermined pose_estimator_area,\n it enables the corresponding pose_estamtor.] +``` + +### Rule helpers + +Rule helpers are auxiliary tools for describing switching rules. + +- [PCD occupancy](#pcd-occupancy) +- [Pose estimator area](#pose-estimator-area) + +#### PCD occupancy + +drawing + +#### Pose estimator area + +The pose_estimator_area is a planar area described by polygon in lanelet2. +The height of the area is meaningless; it judges if the projection of its self-position is contained within the polygon or not. + +drawing + +A sample pose_estimator_area is shown below. The values provided below are placeholders. +To be correctly read, the area should have the type "pose_estimator_specify" and the subtype should be one of ndt, yabloc, eagleye, or artag. + +```xml + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + + + + + + + + + + +``` diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp new file mode 100644 index 0000000000000..c366a7f02f4fe --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ +#include + +#include + +#include + +namespace pose_estimator_arbiter::rule_helper +{ +struct GridKey +{ + const float unit_length = 10.f; + int x, y; + + GridKey() : x(0), y(0) {} + GridKey(float x, float y) : x(std::floor(x / unit_length)), y(std::floor(y / unit_length)) {} + + pcl::PointXYZ get_center_point() const + { + pcl::PointXYZ xyz; + xyz.x = unit_length * (static_cast(x) + 0.5f); + xyz.y = unit_length * (static_cast(y) + 0.5f); + xyz.z = 0.f; + return xyz; + } + + friend bool operator==(const GridKey & one, const GridKey & other) + { + return one.x == other.x && one.y == other.y; + } + friend bool operator!=(const GridKey & one, const GridKey & other) { return !(one == other); } +}; + +} // namespace pose_estimator_arbiter::rule_helper + +// This is for unordered_map and unordered_set +namespace std +{ +template <> +struct hash +{ +public: + size_t operator()(const pose_estimator_arbiter::rule_helper::GridKey & grid) const + { + std::size_t seed = 0; + boost::hash_combine(seed, grid.x); + boost::hash_combine(seed, grid.y); + return seed; + } +}; +} // namespace std + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__GRID_KEY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp new file mode 100644 index 0000000000000..72071b23b467f --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp @@ -0,0 +1,112 @@ +// Copyright 2023 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 "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" + +#include "pose_estimator_arbiter/rule_helper/grid_key.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::rule_helper +{ +PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold) +: pcd_density_upper_threshold_(pcd_density_upper_threshold), + pcd_density_lower_threshold_(pcd_density_lower_threshold) +{ +} + +bool PcdOccupancy::ndt_can_operate( + const geometry_msgs::msg::Point & position, std::string * optional_message) const +{ + if (!kdtree_) { + if (optional_message) { + *optional_message = "pcd is not subscribed yet"; + } + return false; + } + + const pcl::PointXYZ query{ + static_cast(position.x), static_cast(position.y), static_cast(position.z)}; + std::vector indices; + std::vector distances; + const int count = kdtree_->radiusSearch(query, 50, indices, distances, 0); + + static bool last_is_ndt_mode = true; + const bool is_ndt_mode = (last_is_ndt_mode) ? (count > pcd_density_lower_threshold_) + : (count > pcd_density_upper_threshold_); + last_is_ndt_mode = is_ndt_mode; + + std::stringstream ss; + ss << "pcd occupancy: " << count << " > " + << (last_is_ndt_mode ? pcd_density_lower_threshold_ : pcd_density_upper_threshold_); + + if (optional_message) { + *optional_message = ss.str(); + } + + return is_ndt_mode; +} + +visualization_msgs::msg::MarkerArray PcdOccupancy::debug_marker_array() const +{ + visualization_msgs::msg::Marker msg; + msg.ns = "pcd_occupancy"; + msg.id = 0; + msg.header.frame_id = "map"; + msg.scale.set__x(3.0f).set__y(3.0f).set__z(3.f); + msg.color.set__r(1.0f).set__g(1.0f).set__b(0.2f).set__a(1.0f); + + if (occupied_areas_) { + for (auto p : occupied_areas_->points) { + geometry_msgs::msg::Point geometry_point{}; + geometry_point.set__x(p.x).set__y(p.y).set__z(p.z); + msg.points.push_back(geometry_point); + } + } + + visualization_msgs::msg::MarkerArray msg_array; + msg_array.markers.push_back(msg); + + return msg_array; +} + +void PcdOccupancy::init(PointCloud2::ConstSharedPtr msg) +{ + if (kdtree_) { + // already initialized + return; + } + + pcl::PointCloud cloud; + pcl::fromROSMsg(*msg, cloud); + + std::unordered_map grid_point_count; + for (pcl::PointXYZ xyz : cloud) { + grid_point_count[GridKey(xyz.x, xyz.y)] += 1; + } + + occupied_areas_ = std::make_shared>(); + for (const auto [grid, count] : grid_point_count) { + if (count > 50) { + occupied_areas_->push_back(grid.get_center_point()); + } + } + + kdtree_ = pcl::make_shared>(); + kdtree_->setInputCloud(occupied_areas_); +} + +} // namespace pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp new file mode 100644 index 0000000000000..e5eb4ff091a31 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ + +#include + +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +class PcdOccupancy +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold); + + MarkerArray debug_marker_array() const; + void init(PointCloud2::ConstSharedPtr msg); + bool ndt_can_operate( + const geometry_msgs::msg::Point & position, std::string * optional_message = nullptr) const; + +private: + const int pcd_density_upper_threshold_; + const int pcd_density_lower_threshold_; + + pcl::PointCloud::Ptr occupied_areas_{nullptr}; + pcl::KdTreeFLANN::Ptr kdtree_{nullptr}; +}; + +} // namespace pose_estimator_arbiter::rule_helper + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__PCD_OCCUPANCY_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp new file mode 100644 index 0000000000000..4eea34f9ae1ee --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp @@ -0,0 +1,144 @@ +// Copyright 2023 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 "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +using BoostPoint = boost::geometry::model::d2::point_xy; +using BoostPolygon = boost::geometry::model::polygon; + +struct PoseEstimatorArea::Impl +{ + explicit Impl(rclcpp::Logger logger) : logger_(logger) {} + std::multimap bounding_boxes_; + + void init(HADMapBin::ConstSharedPtr msg); + bool within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; + + MarkerArray debug_marker_array() const { return marker_array_; } + +private: + rclcpp::Logger logger_; + MarkerArray marker_array_; +}; + +PoseEstimatorArea::PoseEstimatorArea(const rclcpp::Logger & logger) : logger_(logger) +{ + impl_ = std::make_shared(logger_); +} + +PoseEstimatorArea::PoseEstimatorArea(rclcpp::Node * node) : PoseEstimatorArea(node->get_logger()) +{ +} + +void PoseEstimatorArea::init(HADMapBin::ConstSharedPtr msg) +{ + impl_->init(msg); +} + +bool PoseEstimatorArea::within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const +{ + return impl_->within(point, pose_estimator_name); +} + +PoseEstimatorArea::MarkerArray PoseEstimatorArea::debug_marker_array() const +{ + return impl_->debug_marker_array(); +} + +void PoseEstimatorArea::Impl::init(HADMapBin::ConstSharedPtr msg) +{ + if (!bounding_boxes_.empty()) { + // already initialized + return; + } + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map); + + const auto & polygon_layer = lanelet_map->polygonLayer; + RCLCPP_DEBUG_STREAM(logger_, "Polygon layer size: " << polygon_layer.size()); + + const std::string pose_estimator_specifying_attribute = "pose_estimator_specify"; + for (const auto & polygon : polygon_layer) { + // + const std::string type{polygon.attributeOr(lanelet::AttributeName::Type, "none")}; + RCLCPP_DEBUG_STREAM(logger_, "polygon type: " << type); + if (pose_estimator_specifying_attribute != type) { + continue; + } + + // + const std::string subtype{polygon.attributeOr(lanelet::AttributeName::Subtype, "none")}; + RCLCPP_DEBUG_STREAM(logger_, "polygon sub type: " << subtype); + + // Create a marker for visualization + Marker marker; + marker.type = Marker::LINE_STRIP; + marker.scale.set__x(0.2f).set__y(0.2f).set__z(0.2f); + marker.color.set__r(1.0f).set__g(1.0f).set__b(0.0f).set__a(1.0f); + marker.ns = subtype; + marker.header.frame_id = "map"; + marker.id = marker_array_.markers.size(); + + // Enqueue the vertices of the polygon to bounding box and visualization marker + BoostPolygon poly; + for (const lanelet::ConstPoint3d & p : polygon) { + poly.outer().push_back(BoostPoint(p.x(), p.y())); + + geometry_msgs::msg::Point point_msg; + point_msg.set__x(p.x()).set__y(p.y()).set__z(p.z()); + marker.points.push_back(point_msg); + } + + // Push the first vertex again to enclose the polygon + poly.outer().push_back(poly.outer().front()); + marker.points.push_back(marker.points.front()); + + // Push back the items + bounding_boxes_.emplace(subtype, poly); + marker_array_.markers.push_back(marker); + } +} + +bool PoseEstimatorArea::Impl::within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const +{ + const BoostPoint boost_point(point.x, point.y); + + auto range = bounding_boxes_.equal_range(pose_estimator_name); + + for (auto itr = range.first; itr != range.second; ++itr) { + if (!boost::geometry::disjoint(itr->second, boost_point)) { + return true; + } + } + return false; +} +} // namespace pose_estimator_arbiter::rule_helper diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp new file mode 100644 index 0000000000000..d6901f9be2dbf --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp @@ -0,0 +1,56 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ +#define POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include + +namespace pose_estimator_arbiter::rule_helper +{ +class PoseEstimatorArea +{ + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Marker = visualization_msgs::msg::Marker; + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit PoseEstimatorArea(rclcpp::Node * node); + explicit PoseEstimatorArea(const rclcpp::Logger & logger); + + // This is assumed to be called only once in the vector map callback. + void init(const HADMapBin::ConstSharedPtr msg); + + bool within( + const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const; + + MarkerArray debug_marker_array() const; + +private: + struct Impl; + std::shared_ptr impl_{nullptr}; + rclcpp::Logger logger_; +}; + +} // namespace pose_estimator_arbiter::rule_helper + +#endif // POSE_ESTIMATOR_ARBITER__RULE_HELPER__POSE_ESTIMATOR_AREA_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp new file mode 100644 index 0000000000000..4c3829316230b --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 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 "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" + +#include + +namespace pose_estimator_arbiter::switch_rule +{ +PcdMapBasedRule::PcdMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) +{ + const int pcd_density_upper_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_upper_threshold"); + const int pcd_density_lower_threshold = + tier4_autoware_utils::getOrDeclareParameter(node, "pcd_density_lower_threshold"); + + pcd_occupancy_ = std::make_unique( + pcd_density_upper_threshold, pcd_density_lower_threshold); + + // Register callback + shared_data_->point_cloud_map.register_callback( + [this](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + pcd_occupancy_->init(msg); + }); +} + +PcdMapBasedRule::MarkerArray PcdMapBasedRule::debug_marker_array() +{ + MarkerArray array_msg; + + if (pcd_occupancy_) { + const auto & additional = pcd_occupancy_->debug_marker_array().markers; + array_msg.markers.insert(array_msg.markers.end(), additional.begin(), additional.end()); + } + + return array_msg; +} + +std::unordered_map PcdMapBasedRule::update() +{ + const auto position = shared_data_->localization_pose_cov()->pose.pose.position; + const bool ndt_can_operate = pcd_occupancy_->ndt_can_operate(position); + + if (ndt_can_operate) { + debug_string_ = "Enable ndt: "; + return { + {PoseEstimatorType::ndt, true}, + {PoseEstimatorType::yabloc, false}, + {PoseEstimatorType::eagleye, false}, + {PoseEstimatorType::artag, false}, + }; + } else { + debug_string_ = "Enable yabloc: "; + return { + {PoseEstimatorType::ndt, false}, + {PoseEstimatorType::yabloc, true}, + {PoseEstimatorType::eagleye, false}, + {PoseEstimatorType::artag, false}}; + } +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp new file mode 100644 index 0000000000000..23fd0f812f700 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class PcdMapBasedRule : public BaseSwitchRule +{ +public: + PcdMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + std::unordered_map update() override; + + std::string debug_string() override { return debug_string_; } + + MarkerArray debug_marker_array() override; + +protected: + const std::unordered_set running_estimator_list_; + std::shared_ptr shared_data_{nullptr}; + + std::unique_ptr pcd_occupancy_{nullptr}; + + // Store the reason why which pose estimator is enabled + mutable std::string debug_string_; +}; +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__PCD_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp new file mode 100644 index 0000000000000..094434c62ac9c --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 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 "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" + +#include + +namespace pose_estimator_arbiter::switch_rule +{ +VectorMapBasedRule::VectorMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) +{ + pose_estimator_area_ = std::make_unique(node.get_logger()); + + // Register callback + shared_data_->vector_map.register_callback( + [this](autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -> void { + pose_estimator_area_->init(msg); + }); + + RCLCPP_INFO_STREAM(get_logger(), "VectorMapBasedRule is initialized successfully"); +} + +VectorMapBasedRule::MarkerArray VectorMapBasedRule::debug_marker_array() +{ + MarkerArray array_msg; + + if (pose_estimator_area_) { + const auto & additional = pose_estimator_area_->debug_marker_array().markers; + array_msg.markers.insert(array_msg.markers.end(), additional.begin(), additional.end()); + } + + return array_msg; +} + +std::unordered_map VectorMapBasedRule::update() +{ + const auto ego_position = shared_data_->localization_pose_cov()->pose.pose.position; + + std::unordered_map enable_list; + bool at_least_one_is_enabled = false; + for (const auto & estimator_type : running_estimator_list_) { + const std::string estimator_name{magic_enum::enum_name(estimator_type)}; + const bool result = pose_estimator_area_->within(ego_position, estimator_name); + enable_list.emplace(estimator_type, result); + + at_least_one_is_enabled |= result; + } + if (at_least_one_is_enabled) { + debug_string_ = + "Enable at least one pose_estimators: self vehicle is within the area of at least one " + "pose_estimator_area"; + } else { + debug_string_ = + "Enable no pose_estimator: self vehicle is out of the area of all pose_estimator_area"; + } + RCLCPP_DEBUG(get_logger(), "%s", debug_string_.c_str()); + + return enable_list; +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp new file mode 100644 index 0000000000000..e3360f9692f86 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp @@ -0,0 +1,55 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class VectorMapBasedRule : public BaseSwitchRule +{ +public: + VectorMapBasedRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + std::unordered_map update() override; + + std::string debug_string() override { return debug_string_; } + + MarkerArray debug_marker_array() override; + +protected: + const std::unordered_set running_estimator_list_; + std::shared_ptr shared_data_{nullptr}; + + // Store the reason why which pose estimator is enabled + mutable std::string debug_string_; + + std::unique_ptr pose_estimator_area_{nullptr}; +}; +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__VECTOR_MAP_BASED_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp new file mode 100644 index 0000000000000..5febfa403363e --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -0,0 +1,100 @@ +// Copyright 2023 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 "pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp" + +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + node->declare_parameter("pcd_density_lower_threshold", 1); + node->declare_parameter("pcd_density_upper_threshold", 5); + + const auto running_estimator_list = + std::unordered_set{ + pose_estimator_arbiter::PoseEstimatorType::ndt, + pose_estimator_arbiter::PoseEstimatorType::yabloc, + pose_estimator_arbiter::PoseEstimatorType::eagleye, + pose_estimator_arbiter::PoseEstimatorType::artag}; + + shared_data_ = std::make_shared(); + + rule_ = std::make_shared( + *node, running_estimator_list, shared_data_); + } + + std::shared_ptr node; + std::shared_ptr shared_data_; + std::shared_ptr rule_; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, pcdMapBasedRule) +{ + // Create dummy pcd and set + { + pcl::PointCloud cloud; + for (float x = -10; x < 10.0; x += 0.2) { + for (float y = -10; y < 10.0; y += 0.2) { + cloud.push_back(pcl::PointXYZ(x, y, 0)); + } + } + + using PointCloud2 = sensor_msgs::msg::PointCloud2; + PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + + // Set + shared_data_->point_cloud_map.set_and_invoke(std::make_shared(msg)); + } + + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + auto create_pose = [](double x, double y) -> PoseCovStamped { + PoseCovStamped msg; + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + return msg; + }; + + { + auto position = create_pose(5, 5); + shared_data_->localization_pose_cov.set_and_invoke( + std::make_shared(position)); + auto ret = rule_->update(); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } + + { + auto position = create_pose(100, 100); + shared_data_->localization_pose_cov.set_and_invoke( + std::make_shared(position)); + auto ret = rule_->update(); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } +} diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp new file mode 100644 index 0000000000000..35ed8b04bfcad --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -0,0 +1,106 @@ +// Copyright 2023 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 "pose_estimator_arbiter/rule_helper/grid_key.hpp" +#include "pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp" +#include "pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp" + +#include + +#include + +#include + +#include +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + } + + std::shared_ptr node{nullptr}; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, poseEstimatorArea) +{ + auto create_polygon3d = []() -> lanelet::Polygon3d { + lanelet::Polygon3d polygon; + polygon.setAttribute(lanelet::AttributeName::Type, {"pose_estimator_specify"}); + polygon.setAttribute(lanelet::AttributeName::Subtype, {"ndt"}); + lanelet::Id index = 0; + polygon.push_back(lanelet::Point3d(index++, 0, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 10)); + polygon.push_back(lanelet::Point3d(index++, 0, 10)); + return polygon; + }; + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet_map->add(create_polygon3d()); + + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Point = geometry_msgs::msg::Point; + HADMapBin msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); + + pose_estimator_arbiter::rule_helper::PoseEstimatorArea pose_estimator_area(&(*node)); + pose_estimator_area.init(std::make_shared(msg)); + + EXPECT_TRUE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "ndt")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(5).set__y(5).set__z(0), "yabloc")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "ndt")); + EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "yabloc")); +} + +TEST_F(MockNode, pcdOccupancy) +{ + using pose_estimator_arbiter::rule_helper::PcdOccupancy; + const int pcd_density_upper_threshold = 20; + const int pcd_density_lower_threshold = 10; + + pose_estimator_arbiter::rule_helper::PcdOccupancy pcd_occupancy( + pcd_density_upper_threshold, pcd_density_lower_threshold); + + geometry_msgs::msg::Point point; + std::string message; + + // Since we have not yet given a point cloud, this returns false. + EXPECT_FALSE(pcd_occupancy.ndt_can_operate(point, &message)); +} + +TEST_F(MockNode, gridKey) +{ + using pose_estimator_arbiter::rule_helper::GridKey; + EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.)); + EXPECT_TRUE(GridKey(10., -5.) != GridKey(10., -15.)); + + EXPECT_TRUE(GridKey(10., -5.).get_center_point().x == 15.f); + EXPECT_TRUE(GridKey(10., -5.).get_center_point().y == -5.f); + EXPECT_TRUE(GridKey(10., -5.).get_center_point().z == 0.f); + + std::unordered_set set; + set.emplace(10., -5.); + EXPECT_EQ(set.count(GridKey(10., -5.)), 1ul); + EXPECT_EQ(set.count(GridKey(10., -15.)), 0ul); +} diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp new file mode 100644 index 0000000000000..a0a983e2ad3b7 --- /dev/null +++ b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -0,0 +1,106 @@ +// Copyright 2023 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 "pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp" + +#include + +#include + +#include +#include +#include + +#include + +class MockNode : public ::testing::Test +{ +protected: + virtual void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("test_node"); + + const auto running_estimator_list = + std::unordered_set{ + pose_estimator_arbiter::PoseEstimatorType::ndt, + pose_estimator_arbiter::PoseEstimatorType::yabloc, + pose_estimator_arbiter::PoseEstimatorType::eagleye, + pose_estimator_arbiter::PoseEstimatorType::artag}; + + shared_data_ = std::make_shared(); + + rule_ = std::make_shared( + *node, running_estimator_list, shared_data_); + } + + std::shared_ptr node; + std::shared_ptr shared_data_; + std::shared_ptr rule_; + + virtual void TearDown() { rclcpp::shutdown(); } +}; + +TEST_F(MockNode, vectorMapBasedRule) +{ + // Create dummy lanelet2 and set + { + auto create_polygon3d = []() -> lanelet::Polygon3d { + lanelet::Polygon3d polygon; + polygon.setAttribute(lanelet::AttributeName::Type, {"pose_estimator_specify"}); + polygon.setAttribute(lanelet::AttributeName::Subtype, {"ndt"}); + lanelet::Id index = 0; + polygon.push_back(lanelet::Point3d(index++, 0, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 0)); + polygon.push_back(lanelet::Point3d(index++, 10, 10)); + polygon.push_back(lanelet::Point3d(index++, 0, 10)); + return polygon; + }; + + lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); + lanelet_map->add(create_polygon3d()); + + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + HADMapBin msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); + + // Set + shared_data_->vector_map.set_and_invoke(std::make_shared(msg)); + } + + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + auto create_pose = [](double x, double y) -> PoseCovStamped::ConstSharedPtr { + PoseCovStamped msg; + msg.pose.pose.position.x = x; + msg.pose.pose.position.y = y; + return std::make_shared(msg); + }; + + { + shared_data_->localization_pose_cov.set_and_invoke(create_pose(5, 5)); + auto ret = rule_->update(); + EXPECT_TRUE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } + { + shared_data_->localization_pose_cov.set_and_invoke(create_pose(15, 15)); + auto ret = rule_->update(); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::ndt)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::yabloc)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::eagleye)); + EXPECT_FALSE(ret.at(pose_estimator_arbiter::PoseEstimatorType::artag)); + } +} diff --git a/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml new file mode 100644 index 0000000000000..0a708e3f48988 --- /dev/null +++ b/localization/pose_estimator_arbiter/launch/pose_estimator_arbiter.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/pose_estimator_arbiter/media/architecture.drawio.svg b/localization/pose_estimator_arbiter/media/architecture.drawio.svg new file mode 100644 index 0000000000000..4a7a7fe355775 --- /dev/null +++ b/localization/pose_estimator_arbiter/media/architecture.drawio.svg @@ -0,0 +1,957 @@ + + + + + + + +
+
+
+ legend +
+
+
+ + legend + + + + + + +
+
+
+ pose_estimator_arbiter +
+
+
+ + pose_estimator_arbiter + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + + +
+
+
+ /util/downsampled/pointcloud +
+
+
+ + /util/downsampled/pointcloud + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + + +
+
+
+ /pose_estimator/yabloc/* +
+
+
+ + /pose_estimator/yabloc/* + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ ndt_scan_matcher +
+
+
+ + ndt_scan_matcher + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ yabloc_particle_filter +
+
+
+ + yabloc_particle_filt... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw/relay +
+
+
+ + /sensing/camera... + + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud +
+
+
+ + /sensing/lidar/top... + + + + + + + +
+
+
+ /pose_estimator/eagleye +
+ /pose_with_covariance +
+ /to_relay +
+
+
+ + /pose_estimator/eagleye... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + + +
+
+
+ /map/vector_map, +
+ /map/pointcloud_map, +
+ /initialization_state, +
+ /tf +
+
+
+ + /map/vector_map,... + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud/relay +
+
+
+ + /sensing/lidar/top... + + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw +
+
+
+ + /sensing/camera... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + + +
+
+
+ /sensing/* +
+
+
+ + /sensing/* + + + + + + +
+
+
+ eagleye_stopper +
+
+
+ + eagleye_stopper + + + + + + + +
+
+
+ yabloc_suspend_srv +
+
+
+ + yabloc_suspend_srv + + + + + + +
+
+
+ yabloc_stopper +
+
+
+ + yabloc_stopper + + + + + + +
+
+
+ ndt_stopper +
+
+
+ + ndt_stopper + + + + + + +
+
+
+ switching rule +
+
+
+ + switching rule + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ module +
+
+
+ + module + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ some nodes +
+
+
+ + some nodes + + + + + + +
+
+
+ NDT & YabLoc & Eagleye & AR-Tag +
+
+
+ + + NDT & YabLoc & Eagleye & AR-Tag + + + + + + + + +
+
+
+ base +
+ stopper +
+
+
+ + base... + + + + + + +
+
+
+ stopper +
+
+
+ + stopper + + + + + + + +
+
+
+ Inheritance +
+
+
+ + Inheritance + + + + + +
+
+
+ It has ON/OFF inteface +
+
+
+ + It has ON/OFF inteface + + + + + +
+
+
+ It has pose_estimator- +
+ specific features +
+
+
+ + It has pose_estimator-... + + + + + + +
+
+
+ artag_stopper +
+
+
+ + artag_stopper + + + + + + +
+
+
+ artag +
+
+
+ + artag + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw/relay +
+
+
+ + /sensing/camera... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + + +
+
+
+ /sensing/camera +
+ /traffic_light/image_raw +
+
+
+ + /sensing/camera... + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg b/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg new file mode 100644 index 0000000000000..6a20728e27fd9 --- /dev/null +++ b/localization/pose_estimator_arbiter/media/pcd_occupancy.drawio.svg @@ -0,0 +1,878 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ point cloud +
+
+
+ + point cloud + + + + + + +
+
+
+ non points area +
+
+
+ + non points area + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ occupied grids +
+
+
+ + occupied grids + + + + + + +
+
+
+ not occupied grids +
+
+
+ + not occupied grids + + + + + + + + + + +
+
+
+ surrounding occupied grid=1 +
+
+
+ + surrounding occupied... + + + + + + +
+
+
+ surrounding occupied grids = 6 +
+
+
+ + surrounding occupied... + + + + + + + + + +
+
+
+ divide into grid +
+
+
+ + divide into grid + + + + + +
+
+
+ check occupancy for each grids +
+
+
+ + check occupancy for each grids + + + + + +
+
+
+ + Every period, count the number of occupied grids around the ego. +
+ +
+
+
+ + Every period, count the number of occupied grids around the ego. + + + + + +
+
+
+ ① +
+
+
+ + ① + + + + + +
+
+
+ ② +
+
+
+ + ② + + + + + +
+
+
+ ③ +
+
+
+ + ③ + + + + + +
+
+
+ ④ +
+
+
+ + ④ + + + + + +
+
+
+ grid_size +
+
+
+ + grid_size + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png b/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png new file mode 100644 index 0000000000000..68e6a9ff430e6 Binary files /dev/null and b/localization/pose_estimator_arbiter/media/pose_estimator_area_in_vector_map.png differ diff --git a/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg b/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg new file mode 100644 index 0000000000000..837571474b76a --- /dev/null +++ b/localization/pose_estimator_arbiter/media/single_pose_estimator.drawio.svg @@ -0,0 +1,880 @@ + + + + + + + +
+
+
+ legend +
+
+
+ + legend + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ module +
+
+
+ + module + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ node +
+
+
+ + node + + + + + + +
+
+
+ some nodes +
+
+
+ + some nodes + + + + + + + +
+
+
+ /util/downsampled/pointcloud +
+
+
+ + /util/downsampled/pointcloud + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + + +
+
+
+ /pose_estimator/yabloc/* +
+
+
+ + /pose_estimator/yabloc/* + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ ndt_scan_matcher +
+
+
+ + ndt_scan_matcher + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ yabloc_particle_filter +
+
+
+ + yabloc_particle_filt... + + + + + + + +
+
+
+ /pose_with_covariance +
+
+
+ + /pose_with_covariance + + + + + + + + +
+
+
+ /sensing/lidar/top +
+ /outlier_filtered/pointcloud +
+
+
+ + /sensing/lidar/top... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + + + +
+
+
+ /yabloc/image_processing +
+ /undistorted/image_raw +
+
+
+ + /yabloc/image_processing... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + + +
+
+
+ /sensing/* +
+
+
+ + /sensing/* + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ pointcluod_ +
+ preprocessor +
+
+
+ + pointcluod_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ image_ +
+ preprocessor +
+
+
+ + image_... + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ eagleye +
+
+
+ + eagleye + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + + + + +
+
+
+ only NDT +
+
+
+ + only NDT + + + + + + +
+
+
+ only YabLoc +
+
+
+ + only YabLoc + + + + + + +
+
+
+ only Eagleye +
+
+
+ + only Eagleye + + + + + + +
+
+
+ artag +
+
+
+ + artag + + + + + + + + +
+
+
+ /sensing/camera/ +
+ traffic_light/image_raw +
+
+
+ + /sensing/camera/... + + + + + + +
+
+
+ EKF +
+ (pose_twist_fusion_filter) +
+
+
+ + EKF... + + + + + + + +
+
+
+ /twist_estimator +
+ /twist_with_covariance +
+
+
+ + /twist_estimator... + + + + + + + +
+
+
+ only AR-Tag +
+
+
+ + only AR-Tag + + + + + + +
+
+
+ twist_ +
+ estimator +
+
+
+ + twist_... + + + + + + + +
+
+
+ /pose_estimator +
+ /pose_with_covariance +
+
+
+ + /pose_estimator... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml new file mode 100644 index 0000000000000..b416d42c8617f --- /dev/null +++ b/localization/pose_estimator_arbiter/package.xml @@ -0,0 +1,43 @@ + + + + pose_estimator_arbiter + 0.1.0 + The arbiter of multiple pose estimators + Kento Yabuuchi + Apache License 2.0 + Kento Yabuuchi + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_mapping_msgs + diagnostic_msgs + geometry_msgs + lanelet2_extension + libgoogle-glog-dev + magic_enum + pcl_conversions + pcl_ros + pluginlib + sensor_msgs + std_msgs + std_srvs + tier4_autoware_utils + ublox_msgs + visualization_msgs + yabloc_particle_filter + + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp new file mode 100644 index 0000000000000..9e67dfc063964 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -0,0 +1,100 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ +#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter +{ +class PoseEstimatorArbiter : public rclcpp::Node +{ + using SetBool = std_srvs::srv::SetBool; + using String = std_msgs::msg::String; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Image = sensor_msgs::msg::Image; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + PoseEstimatorArbiter(); + +private: + // Set of running pose estimators specified by ros param `pose_sources` + const std::unordered_set running_estimator_list_; + // Configuration to allow changing the log level by service + const std::unique_ptr logger_configure_; + + // This is passed to several modules (stoppers & rule) so that all modules can access common data + // without passing them as arguments. Also, modules can register subscriber callbacks through + // shared_data, avoiding the need to define duplicate subscribers for each module. + std::shared_ptr shared_data_{nullptr}; + + // Timer callback + rclcpp::TimerBase::SharedPtr timer_; + // Publishers + rclcpp::Publisher::SharedPtr pub_diag_; + rclcpp::Publisher::SharedPtr pub_debug_marker_array_; + rclcpp::Publisher::SharedPtr pub_debug_string_; + // Subscribers for stoppers + rclcpp::Subscription::SharedPtr sub_yabloc_input_; + rclcpp::Subscription::SharedPtr sub_artag_input_; + rclcpp::Subscription::SharedPtr sub_ndt_input_; + rclcpp::Subscription::SharedPtr sub_eagleye_output_; + // Subscribers for switch rules + rclcpp::Subscription::SharedPtr sub_localization_pose_cov_; + rclcpp::Subscription::SharedPtr sub_point_cloud_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_initialization_state_; + + // Stoppers which enable/disable pose estimators + std::unordered_map stoppers_; + // Abstract class to determine which pose estimator should be used + std::shared_ptr switch_rule_{nullptr}; + + // Instruct all stopper to enable/disable + void toggle_all(bool enabled); + // Instruct each stopper to enable/disable + void toggle_each(const std::unordered_map & toggle_list); + + // Load switching rule according to the condition + void load_switch_rule(); + // Publish diagnostic messages + void publish_diagnostics() const; + + // Timer callback + void on_timer(); +}; +} // namespace pose_estimator_arbiter + +#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_ARBITER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp new file mode 100644 index 0000000000000..4fc3fd9b914a6 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_core.cpp @@ -0,0 +1,213 @@ +// Copyright 2023 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 "pose_estimator_arbiter/pose_estimator_arbiter.hpp" +#include "pose_estimator_arbiter/pose_estimator_type.hpp" +#include "pose_estimator_arbiter/stopper/stopper_artag.hpp" +#include "pose_estimator_arbiter/stopper/stopper_eagleye.hpp" +#include "pose_estimator_arbiter/stopper/stopper_ndt.hpp" +#include "pose_estimator_arbiter/stopper/stopper_yabloc.hpp" +#include "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" + +#include + +namespace pose_estimator_arbiter +{ +// Parses ros param to get the estimator set that is running +static std::unordered_set parse_estimator_name_args( + const std::vector & arg, const rclcpp::Logger & logger) +{ + std::unordered_set running_estimator_list; + for (const auto & estimator_name : arg) { + const auto estimator = magic_enum::enum_cast(estimator_name); + + if (estimator.has_value()) { + running_estimator_list.insert(estimator.value()); + } else { + RCLCPP_ERROR_STREAM(logger, "invalid pose_estimator_name is specified: " << estimator_name); + } + } + + return running_estimator_list; +} + +PoseEstimatorArbiter::PoseEstimatorArbiter() +: Node("pose_estimator_arbiter"), + running_estimator_list_(parse_estimator_name_args( + declare_parameter>("pose_sources"), get_logger())), + logger_configure_(std::make_unique(this)) +{ + // Shared data + shared_data_ = std::make_shared(); + + // Publisher + pub_diag_ = create_publisher("/diagnostics", 10); + pub_debug_string_ = create_publisher("~/debug/string", 10); + pub_debug_marker_array_ = create_publisher("~/debug/marker_array", 10); + + // Define function to get running pose_estimator + const std::set running_estimator_set( + running_estimator_list_.begin(), running_estimator_list_.end()); + const auto is_running = [running_estimator_set](const PoseEstimatorType type) -> bool { + return running_estimator_set.count(type) != 0; + }; + + // QoS + const rclcpp::QoS sensor_qos = rclcpp::SensorDataQoS(); + const rclcpp::QoS latch_qos = rclcpp::QoS(1).transient_local().reliable(); + + // Create stoppers & subscribers + if (is_running(PoseEstimatorType::ndt)) { + stoppers_.emplace( + PoseEstimatorType::ndt, std::make_shared(this, shared_data_)); + sub_ndt_input_ = create_subscription( + "~/input/ndt/pointcloud", sensor_qos, shared_data_->ndt_input_points.create_callback()); + } + if (is_running(PoseEstimatorType::yabloc)) { + stoppers_.emplace( + PoseEstimatorType::yabloc, std::make_shared(this, shared_data_)); + sub_yabloc_input_ = create_subscription( + "~/input/yabloc/image", sensor_qos, shared_data_->yabloc_input_image.create_callback()); + } + if (is_running(PoseEstimatorType::eagleye)) { + stoppers_.emplace( + PoseEstimatorType::eagleye, std::make_shared(this, shared_data_)); + sub_eagleye_output_ = create_subscription( + "~/input/eagleye/pose_with_covariance", 5, /* this is not sensor topic */ + shared_data_->eagleye_output_pose_cov.create_callback()); + } + if (is_running(PoseEstimatorType::artag)) { + stoppers_.emplace( + PoseEstimatorType::artag, std::make_shared(this, shared_data_)); + sub_artag_input_ = create_subscription( + "~/input/artag/image", sensor_qos, shared_data_->artag_input_image.create_callback()); + } + + // Subscribers for switch rule + { + sub_localization_pose_cov_ = create_subscription( + "~/input/pose_with_covariance", 5, shared_data_->localization_pose_cov.create_callback()); + sub_point_cloud_map_ = create_subscription( + "~/input/pointcloud_map", latch_qos, shared_data_->point_cloud_map.create_callback()); + sub_vector_map_ = create_subscription( + "~/input/vector_map", latch_qos, shared_data_->vector_map.create_callback()); + sub_initialization_state_ = create_subscription( + "~/input/initialization_state", latch_qos, + shared_data_->initialization_state.create_callback()); + } + + // Load switching rule + load_switch_rule(); + + // Timer callback + auto on_timer = std::bind(&PoseEstimatorArbiter::on_timer, this); + timer_ = + rclcpp::create_timer(this, this->get_clock(), rclcpp::Rate(1).period(), std::move(on_timer)); + + // Enable all pose estimators at the first + toggle_all(true); +} + +void PoseEstimatorArbiter::load_switch_rule() +{ + // NOTE: In the future, some rule will be laid below + RCLCPP_INFO_STREAM(get_logger(), "load default switching rule"); + switch_rule_ = + std::make_shared(*this, running_estimator_list_, shared_data_); +} + +void PoseEstimatorArbiter::toggle_each( + const std::unordered_map & toggle_list) +{ + for (auto [type, stopper] : stoppers_) { + RCLCPP_DEBUG_STREAM( + get_logger(), magic_enum::enum_name(type) << " : " << std::boolalpha << toggle_list.at(type)); + + // If the rule implementation is perfect, toggle_list should contains all pose_estimator_type. + if (toggle_list.count(type) == 0) { + RCLCPP_ERROR_STREAM( + get_logger(), magic_enum::enum_name(type) << " is not included in toggle_list."); + continue; + } + + // Enable or disable according to toggle_list + if (toggle_list.at(type)) { + stopper->enable(); + } else { + stopper->disable(); + } + } +} + +void PoseEstimatorArbiter::toggle_all(bool enabled) +{ + // Create toggle_list + std::unordered_map toggle_list; + for (auto [type, stopper] : stoppers_) { + toggle_list.emplace(type, enabled); + } + + // Apply toggle_list + toggle_each(toggle_list); +} + +void PoseEstimatorArbiter::publish_diagnostics() const +{ + diagnostic_msgs::msg::DiagnosticStatus diag_status; + + // Temporary implementation + { + diag_status.name = "localization: " + std::string(this->get_name()); + diag_status.hardware_id = this->get_name(); + + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status.message = "OK"; + + // TODO(KYabuuchi) : Add more details + diagnostic_msgs::msg::KeyValue key_value_msg; + key_value_msg.key = "state"; + key_value_msg.value = "Further details have not been implemented yet."; + diag_status.values.push_back(key_value_msg); + } + + DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status); + + pub_diag_->publish(diag_msg); +} + +void PoseEstimatorArbiter::on_timer() +{ + // Toggle each stopper status + if (switch_rule_) { + const auto toggle_list = switch_rule_->update(); + toggle_each(toggle_list); + + // Publish std_msg::String for debug + pub_debug_string_->publish(String().set__data(switch_rule_->debug_string())); + // Publish visualization_msgs::MarkerArray for debug + pub_debug_marker_array_->publish(switch_rule_->debug_marker_array()); + + } else { + RCLCPP_WARN_STREAM( + get_logger(), "switch_rule is not activated. Therefore, enable all pose_estimators"); + toggle_all(true); + } + + // Publish diagnostic results periodically + publish_diagnostics(); +} + +} // namespace pose_estimator_arbiter diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp new file mode 100644 index 0000000000000..e48507948520b --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter_node.cpp @@ -0,0 +1,31 @@ +// Copyright 2023 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 "pose_estimator_arbiter/pose_estimator_arbiter.hpp" + +#include + +int main(int argc, char * argv[]) +{ + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp new file mode 100644 index 0000000000000..edf12537198a3 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp @@ -0,0 +1,23 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ +#define POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ + +namespace pose_estimator_arbiter +{ +enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; +} + +#endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp new file mode 100644 index 0000000000000..b9c3bd45c9e24 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp @@ -0,0 +1,101 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ +#define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace pose_estimator_arbiter +{ +template +struct CallbackInvokingVariable +{ + CallbackInvokingVariable() {} + + explicit CallbackInvokingVariable(T initial_data) : value(initial_data) {} + + // Set data and invoke all callbacks + void set_and_invoke(T new_value) + { + value = new_value; + + // Call all callbacks with the new value + for (const auto & callback : callbacks) { + callback(value.value()); + } + } + + // Same as std::optional::has_value() + bool has_value() const { return value.has_value(); } + + // Same as std::optional::value() + const T operator()() const { return value.value(); } + + // Register callback function which is invoked when set_and_invoke() is called + void register_callback(std::function callback) const { callbacks.push_back(callback); } + + // Create subscription callback function which is used as below: + // auto subscriber = create_subscription("topic_name", 10, + // callback_invoking_variable.create_callback()); + auto create_callback() + { + return std::bind(&CallbackInvokingVariable::set_and_invoke, this, std::placeholders::_1); + } + +private: + // The latest data + std::optional value{std::nullopt}; + + // These functions are expected not to change the value variable + mutable std::vector> callbacks; +}; + +// This structure is handed to several modules as shared_ptr so that all modules can access data. +struct SharedData +{ +public: + using Image = sensor_msgs::msg::Image; + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + + SharedData() {} + + // Used for stoppers + CallbackInvokingVariable eagleye_output_pose_cov; + CallbackInvokingVariable artag_input_image; + CallbackInvokingVariable ndt_input_points; + CallbackInvokingVariable yabloc_input_image; + + // Used for switch rule + CallbackInvokingVariable localization_pose_cov; + CallbackInvokingVariable point_cloud_map; + CallbackInvokingVariable vector_map; + CallbackInvokingVariable initialization_state{ + std::make_shared( + InitializationState{}.set__state(InitializationState::UNINITIALIZED))}; +}; + +} // namespace pose_estimator_arbiter +#endif // POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp new file mode 100644 index 0000000000000..fc2242a0b27e9 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class BaseStopper +{ +public: + using SharedPtr = std::shared_ptr; + + explicit BaseStopper(rclcpp::Node * node, const std::shared_ptr shared_data) + : logger_(node->get_logger()), shared_data_(shared_data) + { + } + + void enable() { set_enable(true); } + void disable() { set_enable(false); } + + virtual void set_enable(bool enabled) = 0; + +protected: + rclcpp::Logger logger_; + std::shared_ptr shared_data_{nullptr}; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__BASE_STOPPER_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp new file mode 100644 index 0000000000000..72a3f5412add0 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp @@ -0,0 +1,57 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ + +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include +#include + +#include +namespace pose_estimator_arbiter::stopper +{ +class StopperArTag : public BaseStopper +{ + using Image = sensor_msgs::msg::Image; + using SetBool = std_srvs::srv::SetBool; + +public: + explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + ar_tag_is_enabled_ = true; + pub_image_ = node->create_publisher("~/output/artag/image", rclcpp::SensorDataQoS()); + + // Register callback + shared_data_->artag_input_image.register_callback([this](Image::ConstSharedPtr msg) -> void { + if (ar_tag_is_enabled_) { + pub_image_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { ar_tag_is_enabled_ = enabled; } + +protected: + rclcpp::CallbackGroup::SharedPtr service_callback_group_; + +private: + bool ar_tag_is_enabled_; + rclcpp::Publisher::SharedPtr pub_image_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_ARTAG_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp new file mode 100644 index 0000000000000..12bbe8c9769a1 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperEagleye : public BaseStopper +{ + using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +public: + explicit StopperEagleye(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + eagleye_is_enabled_ = true; + pub_pose_ = node->create_publisher("~/output/eagleye/pose_with_covariance", 5); + + // Register callback + shared_data_->eagleye_output_pose_cov.register_callback( + [this](PoseCovStamped::ConstSharedPtr msg) -> void { + if (eagleye_is_enabled_) { + pub_pose_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { eagleye_is_enabled_ = enabled; } + +private: + bool eagleye_is_enabled_; + rclcpp::Publisher::SharedPtr pub_pose_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_EAGLEYE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp new file mode 100644 index 0000000000000..dacea02f77bde --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperNdt : public BaseStopper +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + +public: + explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + ndt_is_enabled_ = true; + pub_pointcloud_ = node->create_publisher( + "~/output/ndt/pointcloud", rclcpp::SensorDataQoS().keep_last(10)); + + // Register callback + shared_data_->ndt_input_points.register_callback( + [this](PointCloud2::ConstSharedPtr msg) -> void { + if (ndt_is_enabled_) { + pub_pointcloud_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override { ndt_is_enabled_ = enabled; } + +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_; + bool ndt_is_enabled_; +}; +} // namespace pose_estimator_arbiter::stopper + +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_NDT_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp new file mode 100644 index 0000000000000..2cd8b66f4ffd4 --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +#define POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ +#include "pose_estimator_arbiter/stopper/base_stopper.hpp" + +#include +#include + +#include + +namespace pose_estimator_arbiter::stopper +{ +class StopperYabLoc : public BaseStopper +{ + using Image = sensor_msgs::msg::Image; + using SetBool = std_srvs::srv::SetBool; + +public: + explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr shared_data) + : BaseStopper(node, shared_data) + { + yabloc_is_enabled_ = true; + pub_image_ = node->create_publisher("~/output/yabloc/image", rclcpp::SensorDataQoS()); + + service_callback_group_ = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + // Prepare suspend service server + using namespace std::literals::chrono_literals; + enable_service_client_ = node->create_client( + "~/yabloc_trigger_srv", rmw_qos_profile_services_default, service_callback_group_); + while (!enable_service_client_->wait_for_service(1s) && rclcpp::ok()) { + RCLCPP_INFO( + node->get_logger(), "Waiting for service : %s", enable_service_client_->get_service_name()); + } + + // Register callback + shared_data_->yabloc_input_image.register_callback([this](Image::ConstSharedPtr msg) -> void { + if (yabloc_is_enabled_) { + pub_image_->publish(*msg); + } + }); + } + + void set_enable(bool enabled) override + { + if (yabloc_is_enabled_ != enabled) { + request_service(enabled); + } + yabloc_is_enabled_ = enabled; + } + +protected: + rclcpp::CallbackGroup::SharedPtr service_callback_group_; + + void request_service(bool flag) + { + using namespace std::literals::chrono_literals; + auto request = std::make_shared(); + request->data = flag; + + auto result_future = enable_service_client_->async_send_request(request); + std::future_status status = result_future.wait_for(1000ms); + if (status == std::future_status::ready) { + RCLCPP_DEBUG_STREAM(logger_, "stopper_yabloc dis/enabling service exited successfully"); + } else { + RCLCPP_ERROR_STREAM(logger_, "stopper_yabloc dis/enabling service exited unexpectedly"); + } + } + +private: + bool yabloc_is_enabled_; + rclcpp::Client::SharedPtr enable_service_client_; + rclcpp::Publisher::SharedPtr pub_image_; +}; +} // namespace pose_estimator_arbiter::stopper +#endif // POSE_ESTIMATOR_ARBITER__STOPPER__STOPPER_YABLOC_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp new file mode 100644 index 0000000000000..5770f18efa32c --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ + +#include "pose_estimator_arbiter/pose_estimator_type.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class BaseSwitchRule +{ +protected: + using MarkerArray = visualization_msgs::msg::MarkerArray; + +public: + explicit BaseSwitchRule(rclcpp::Node & node) + : logger_ptr_(std::make_shared(node.get_logger())) + { + } + + virtual ~BaseSwitchRule() = default; + virtual std::unordered_map update() = 0; + virtual std::string debug_string() { return std::string{}; } + virtual MarkerArray debug_marker_array() { return MarkerArray{}; } + +protected: + rclcpp::Logger get_logger() const { return *logger_ptr_; } + std::shared_ptr logger_ptr_{nullptr}; +}; + +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__BASE_SWITCH_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp new file mode 100644 index 0000000000000..2b2e325c3d94b --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp @@ -0,0 +1,43 @@ +// Copyright 2023 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 "pose_estimator_arbiter/switch_rule/enable_all_rule.hpp" + +#include + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +EnableAllRule::EnableAllRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr) +: BaseSwitchRule(node), running_estimator_list_(running_estimator_list) +{ +} + +std::unordered_map EnableAllRule::update() +{ + return { + {PoseEstimatorType::ndt, true}, + {PoseEstimatorType::yabloc, true}, + {PoseEstimatorType::eagleye, true}, + {PoseEstimatorType::artag, true}, + }; +} + +} // namespace pose_estimator_arbiter::switch_rule diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp new file mode 100644 index 0000000000000..568226985b2ff --- /dev/null +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 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 POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ +#define POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ + +#include "pose_estimator_arbiter/shared_data.hpp" +#include "pose_estimator_arbiter/switch_rule/base_switch_rule.hpp" + +#include +#include +#include +#include + +namespace pose_estimator_arbiter::switch_rule +{ +class EnableAllRule : public BaseSwitchRule +{ +public: + EnableAllRule( + rclcpp::Node & node, const std::unordered_set & running_estimator_list, + const std::shared_ptr shared_data); + + virtual ~EnableAllRule() = default; + + std::unordered_map update() override; + +protected: + const std::unordered_set running_estimator_list_; +}; + +} // namespace pose_estimator_arbiter::switch_rule + +#endif // POSE_ESTIMATOR_ARBITER__SWITCH_RULE__ENABLE_ALL_RULE_HPP_ diff --git a/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py b/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py new file mode 100644 index 0000000000000..c419fb68e0571 --- /dev/null +++ b/localization/pose_estimator_arbiter/test/test_pose_estimator_arbiter.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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. + +import os +import time +import unittest + +from ament_index_python import get_package_share_directory +from geometry_msgs.msg import PoseWithCovarianceStamped +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSProfile +from rclpy.qos import ReliabilityPolicy +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import SetBool + +# This test confirms that all topics are relayed by arbiter. + + +@pytest.mark.launch_test +def generate_test_description(): + test_pose_estimator_arbiter_launch_file = os.path.join( + get_package_share_directory("pose_estimator_arbiter"), + "launch", + "pose_estimator_arbiter.launch.xml", + ) + + pose_estimator_arbiter = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_pose_estimator_arbiter_launch_file), + launch_arguments={ + "pose_sources": "[ndt, yabloc, eagleye, artag]", + "input_pointcloud": "/sensing/lidar/top/pointcloud", + }.items(), + ) + + return launch.LaunchDescription( + [ + pose_estimator_arbiter, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestPoseEstimatorArbiter(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + + def tearDown(self): + self.test_node.destroy_node() + + def spin_for(self, duration_sec): + end_time = time.time() + duration_sec + while time.time() < end_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + def yabloc_suspend_service_callback(self, srv): + pass + + def publish_input_topics(self): + self.pub_ndt_input.publish(PointCloud2()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.pub_yabloc_input.publish(Image()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.pub_eagleye_input.publish(PoseWithCovarianceStamped()) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + def create_publishers_and_subscribers(self): + # Publisher + qos_profile = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT) + + self.pub_ndt_input = self.test_node.create_publisher( + PointCloud2, "/sensing/lidar/top/pointcloud", qos_profile + ) + # pub_yabloc_input is used for both yabloc and artag + self.pub_yabloc_input = self.test_node.create_publisher( + Image, "/sensing/camera/traffic_light/image_raw", qos_profile + ) + self.pub_eagleye_input = self.test_node.create_publisher( + PoseWithCovarianceStamped, + "/localization/pose_estimator/eagleye/pose_with_covariance/to_relay", + QoSProfile(depth=10), + ) + + # Subscriber + self.test_node.create_subscription( + PointCloud2, + "/sensing/lidar/top/pointcloud/relay", + lambda msg: self.ndt_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + Image, + "/sensing/camera/traffic_light/image_raw/yabloc_relay", + lambda msg: self.yabloc_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + Image, + "/sensing/camera/traffic_light/image_raw/artag_relay", + lambda msg: self.artag_relayed.append(msg.header), + qos_profile, + ) + self.test_node.create_subscription( + PoseWithCovarianceStamped, + "/localization/pose_estimator/pose_with_covariance", + lambda msg: self.eagleye_relayed.append(msg.header), + qos_profile, + ) + + def test_node_link(self): + # The arbiter waits for the service to start, so here it instantiates a meaningless service server. + self.test_node.create_service( + SetBool, + "/localization/pose_estimator/yabloc/pf/yabloc_trigger_srv", + self.yabloc_suspend_service_callback, + ) + + # Define subscription buffer + self.ndt_relayed = [] + self.yabloc_relayed = [] + self.artag_relayed = [] + self.eagleye_relayed = [] + + # Create publishers and subscribers + self.create_publishers_and_subscribers() + + # Wait 0.5 second for node to be ready + self.spin_for(0.5) + + # Publish dummy input topics + for _ in range(10): + self.publish_input_topics() + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + # Wait 0.5 second for all topics to be subscribed + self.spin_for(0.5) + + # Confirm both topics are relayed + # In reality, 10topics should be received, but with a margin, 5 is used as the threshold. + self.assertGreater(len(self.ndt_relayed), 5) + self.assertGreater(len(self.yabloc_relayed), 5) + self.assertGreater(len(self.eagleye_relayed), 5) + self.assertGreater(len(self.artag_relayed), 5) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/localization/pose_estimator_arbiter/test/test_shared_data.cpp b/localization/pose_estimator_arbiter/test/test_shared_data.cpp new file mode 100644 index 0000000000000..3c1fa50b502a1 --- /dev/null +++ b/localization/pose_estimator_arbiter/test/test_shared_data.cpp @@ -0,0 +1,59 @@ +// Copyright 2023 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 "pose_estimator_arbiter/shared_data.hpp" + +#include + +TEST(SharedData, callback_invoked_correctly) +{ + pose_estimator_arbiter::CallbackInvokingVariable variable; + + // register callback + bool callback_invoked = false; + int processed_value = 0; + variable.register_callback([&](const int & value) { + callback_invoked = true; + processed_value = 2 * value; + }); + + EXPECT_FALSE(variable.has_value()); + EXPECT_FALSE(callback_invoked); + EXPECT_EQ(processed_value, 0); + + // set value and invoke callback + const int expected_value = 10; + variable.set_and_invoke(expected_value); + + EXPECT_TRUE(variable.has_value()); + EXPECT_TRUE(callback_invoked); + EXPECT_TRUE(variable() == expected_value); + EXPECT_TRUE(processed_value == 2 * expected_value); +} + +TEST(SharedData, multiple_callback_invoked_correctly) +{ + pose_estimator_arbiter::CallbackInvokingVariable variable; + + // register callback + int callback_invoked_num = 0; + variable.register_callback([&](const int &) { callback_invoked_num++; }); + variable.register_callback([&](const int &) { callback_invoked_num++; }); + variable.register_callback([&](const int &) { callback_invoked_num++; }); + + // set value and invoke callback + variable.set_and_invoke(10); + + EXPECT_EQ(callback_invoked_num, 3); +} diff --git a/localization/pose_initializer/README.md b/localization/pose_initializer/README.md index 961f3a8e3293c..3063e13c10226 100644 --- a/localization/pose_initializer/README.md +++ b/localization/pose_initializer/README.md @@ -22,6 +22,8 @@ This node depends on the map height fitter library. | `gnss_enabled` | bool | If true, use the GNSS pose when no pose is specified. | | `gnss_pose_timeout` | bool | The duration that the GNSS pose is valid. | +{{ json_to_markdown("localization/pose_initializer/schema/pose_initializer.schema.json") }} + ### Services | Name | Type | Description | diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index a05cc7c35cb1a..88acbfb52611d 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -2,6 +2,11 @@ ros__parameters: gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] + ekf_enabled: $(var ekf_enabled) + gnss_enabled: $(var gnss_enabled) + yabloc_enabled: $(var yabloc_enabled) + ndt_enabled: $(var ndt_enabled) + stop_check_enabled: $(var stop_check_enabled) # from gnss gnss_particle_covariance: diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 43e8d48c4fda0..a0fdde13c65c5 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -9,13 +9,7 @@ - - - - - - - + diff --git a/localization/pose_initializer/schema/pose_initializer.schema.json b/localization/pose_initializer/schema/pose_initializer.schema.json new file mode 100644 index 0000000000000..14a286f059030 --- /dev/null +++ b/localization/pose_initializer/schema/pose_initializer.schema.json @@ -0,0 +1,85 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pose Initializer Node", + "type": "object", + "definitions": { + "pose_initializer": { + "type": "object", + "properties": { + "gnss_pose_timeout": { + "type": "number", + "description": "The duration that the GNSS pose is valid. [sec]", + "default": "3.0", + "minimum": 0.0 + }, + "stop_check_enabled": { + "type": "string", + "description": "If true, initialization is accepted only when the vehicle is stopped.", + "default": "true" + }, + "stop_check_duration": { + "type": "number", + "description": "The duration used for the stop check above. [sec]", + "default": "3.0", + "minimum": 0.0 + }, + "ekf_enabled": { + "type": "string", + "description": "If true, EKF localizer is activated.", + "default": "true" + }, + "gnss_enabled": { + "type": "string", + "description": "If true, use the GNSS pose when no pose is specified.", + "default": "true" + }, + "yabloc_enabled": { + "type": "string", + "description": "If true, YabLocModule is used.", + "default": "true" + }, + "ndt_enabled": { + "type": "string", + "description": "If true, the pose will be estimated by NDT scan matcher, otherwise it is passed through.", + "default": "true" + }, + "gnss_particle_covariance": { + "type": "array", + "description": "gnss particle covariance", + "default": "[1.0, 0.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]" + }, + "output_pose_covariance": { + "type": "array", + "description": "output pose covariance", + "default": "[1.0, 0.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,\n 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]" + } + }, + "required": [ + "gnss_pose_timeout", + "stop_check_duration", + "ekf_enabled", + "gnss_enabled", + "yabloc_enabled", + "ndt_enabled", + "stop_check_enabled", + "gnss_particle_covariance", + "output_pose_covariance" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pose_initializer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 93b9546be0a36..59f23eacb2fb3 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -13,4 +13,5 @@ ament_target_dependencies(twist2accel) ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/localization/twist2accel/README.md b/localization/twist2accel/README.md index ec73c34052f99..5a540dca895d4 100644 --- a/localization/twist2accel/README.md +++ b/localization/twist2accel/README.md @@ -21,10 +21,7 @@ This package is responsible for estimating acceleration using the output of `ekf ## Parameters -| Name | Type | Description | -| -------------------- | ------ | ------------------------------------------------------------------------- | -| `use_odom` | bool | use odometry if true, else use twist input (default: true) | -| `accel_lowpass_gain` | double | lowpass gain for lowpass filter in estimating acceleration (default: 0.9) | +{{ json_to_markdown("localization/twist2accel/schema/twist2accel.schema.json") }} ## Future work diff --git a/localization/twist2accel/config/twist2accel.param.yaml b/localization/twist2accel/config/twist2accel.param.yaml new file mode 100644 index 0000000000000..e58e029248253 --- /dev/null +++ b/localization/twist2accel/config/twist2accel.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + use_odom: true + accel_lowpass_gain: 0.9 diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index 47b8a95d13a08..c4c9a3ed50bfc 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -1,6 +1,6 @@ - - + + @@ -8,7 +8,6 @@ - - + diff --git a/localization/twist2accel/schema/twist2accel.schema.json b/localization/twist2accel/schema/twist2accel.schema.json new file mode 100644 index 0000000000000..6b3c2bd094166 --- /dev/null +++ b/localization/twist2accel/schema/twist2accel.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for twist2accel Nodes", + "type": "object", + "definitions": { + "twist2accel": { + "type": "object", + "properties": { + "use_odom": { + "type": "boolean", + "default": true, + "description": "use odometry if true, else use twist input." + }, + "accel_lowpass_gain": { + "type": "number", + "default": 0.9, + "minimum": 0.0, + "description": "lowpass gain for lowpass filter in estimating acceleration." + } + }, + "required": ["use_odom", "accel_lowpass_gain"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/twist2accel" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index a8a7d46f57056..68019f27e95fe 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -35,8 +35,8 @@ Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOption pub_accel_ = create_publisher("output/accel", 1); prev_twist_ptr_ = nullptr; - accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain", 0.5); - use_odom_ = declare_parameter("use_odom", true); + accel_lowpass_gain_ = declare_parameter("accel_lowpass_gain"); + use_odom_ = declare_parameter("use_odom"); lpf_alx_ptr_ = std::make_shared(accel_lowpass_gain_); lpf_aly_ptr_ = std::make_shared(accel_lowpass_gain_); diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 1160f1b314b99..6368305bdbfad 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -32,11 +32,7 @@ It estimates the height and tilt of the ground from lanelet2. ### Parameters -| Name | Type | Description | -| ----------------- | ---- | -------------------------------------------------------- | -| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. | -| `K` | int | parameter for nearest k search | -| `R` | int | parameter for radius search | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ground_server.schema.json") }} ## ll2_decomposer @@ -63,8 +59,4 @@ This node extracts the elements related to the road surface markings and yabloc ### Parameters -| Name | Type | Description | -| --------------------- | ---------------- | ---------------------------------------------------------------------- | -| `road_marking_labels` | vector\ | This label is used to extract the road surface markings from lanelet2. | -| `sign_board_labels` | vector\ | This label is used to extract the traffic sign boards from lanelet2. | -| `bounding_box_labels` | vector\ | This label is used to extract the bounding boxes from lanelet2. | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json") }} diff --git a/localization/yabloc/yabloc_common/schema/ground_server.schema.json b/localization/yabloc/yabloc_common/schema/ground_server.schema.json new file mode 100644 index 0000000000000..a9b0674cedd1c --- /dev/null +++ b/localization/yabloc/yabloc_common/schema/ground_server.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ground_server", + "type": "object", + "definitions": { + "ground_server": { + "type": "object", + "properties": { + "force_zero_tilt": { + "type": "boolean", + "description": "if true, the tilt is always determined to be horizontal", + "default": false + }, + "K": { + "type": "number", + "description": "the number of neighbors for ground search on a map", + "default": 50 + }, + "R": { + "type": "number", + "description": "radius for ground search on a map [m]", + "default": 10 + } + }, + "required": ["force_zero_tilt", "K", "R"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ground_server" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json b/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json new file mode 100644 index 0000000000000..fa5a7b37464f3 --- /dev/null +++ b/localization/yabloc/yabloc_common/schema/ll2_decomposer.schema.json @@ -0,0 +1,51 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ll2_decomposer", + "type": "object", + "definitions": { + "ll2_decomposer": { + "type": "object", + "properties": { + "road_marking_labels": { + "type": "array", + "description": "line string types that indicating road surface markings in lanelet2", + "default": [ + "cross_walk", + "zebra_marking", + "line_thin", + "line_thick", + "pedestrian_marking", + "stop_line", + "road_border" + ] + }, + "sign_board_labels": { + "type": "array", + "description": "line string types that indicating traffic sign boards in lanelet2", + "default": ["sign-board"] + }, + "bounding_box_labels": { + "type": "array", + "description": "line string types that indicating not mapped areas in lanelet2", + "default": ["none"] + } + }, + "required": ["road_marking_labels", "sign_board_labels", "bounding_box_labels"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ll2_decomposer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index 85e1d408a1b4b..9816a02c48121 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -53,15 +53,7 @@ This node extract road surface region by [graph-based-segmentation](https://docs ### Parameters -| Name | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------ | -| `target_height_ratio` | double | height on the image to retrieve the candidate road surface | -| `target_candidate_box_width` | int | size of the square area to search for candidate road surfaces | -| `pickup_additional_graph_segment` | bool | if this is true, additional regions of similar color are retrieved | -| `similarity_score_threshold` | double | threshold for picking up additional areas | -| `sigma` | double | parameters for cv::ximgproc::segmentation | -| `k` | double | parameters for cv::ximgproc::segmentation | -| `min_size` | double | parameters for cv::ximgproc::segmentation | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json") }} ## segment_filter @@ -89,14 +81,7 @@ This is a node that integrates the results of graph_segment and lsd to extract r ### Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | ------------------------------------------------------------------- | -| `min_segment_length` | double | min length threshold (if it is negative, it is unlimited) | -| `max_segment_distance` | double | max distance threshold (if it is negative, it is unlimited) | -| `max_lateral_distance` | double | max lateral distance threshold (if it is negative, it is unlimited) | -| `publish_image_with_segment_for_debug` | bool | toggle whether to publish the filtered line segment for debug | -| `max_range` | double | range of debug projection visualization | -| `image_size` | int | image size of debug projection visualization | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json") }} ## undistort @@ -127,11 +112,7 @@ This is to avoid redundant decompression within Autoware. ### Parameters -| Name | Type | Description | -| ------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `use_sensor_qos` | bool | where to use sensor qos or not | -| `width` | int | resized image width size | -| `override_frame_id` | string | value for overriding the camera's frame_id. if blank, frame_id of static_tf is not overwritten | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/undistort.schema.json") }} #### about tf_static overriding diff --git a/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json b/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json new file mode 100644 index 0000000000000..c0bf96dd0de7a --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for graph_segment", + "type": "object", + "definitions": { + "graph_segment": { + "type": "object", + "properties": { + "target_height_ratio": { + "type": "number", + "description": "height on the image to retrieve the candidate road surface", + "default": 0.85 + }, + "target_candidate_box_width": { + "type": "number", + "description": "size of the square area to search for candidate road surfaces", + "default": 15 + }, + "pickup_additional_graph_segment": { + "type": "boolean", + "description": "if this is true, additional regions of similar color are retrieved", + "default": true + }, + "similarity_score_threshold": { + "type": "number", + "description": "threshold for picking up additional areas", + "default": 0.8 + }, + "sigma": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 0.5 + }, + "k": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 300.0 + }, + "min_size": { + "type": "number", + "description": "parameter for cv::ximgproc::segmentation::GraphSegmentation", + "default": 100.0 + } + }, + "required": [ + "target_height_ratio", + "target_candidate_box_width", + "pickup_additional_graph_segment", + "similarity_score_threshold", + "sigma", + "k", + "min_size" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/graph_segment" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json b/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json new file mode 100644 index 0000000000000..bfe74ea80569c --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for segment_filter", + "type": "object", + "definitions": { + "segment_filter": { + "type": "object", + "properties": { + "min_segment_length": { + "type": "number", + "description": "min length threshold (if it is negative, it is unlimited)", + "default": 1.5 + }, + "max_segment_distance": { + "type": "number", + "description": "max distance threshold (if it is negative, it is unlimited)", + "default": 30.0 + }, + "max_lateral_distance": { + "type": "number", + "description": "max lateral distance threshold (if it is negative, it is unlimited)", + "default": 10.0 + }, + "publish_image_with_segment_for_debug": { + "type": "boolean", + "description": "toggle whether to publish the filtered line segment for debug", + "default": true + }, + "max_range": { + "type": "number", + "description": "range of debug projection visualization", + "default": 20.0 + }, + "image_size": { + "type": "number", + "description": "image size of debug projection visualization", + "default": 800 + } + }, + "required": [ + "min_segment_length", + "max_segment_distance", + "max_lateral_distance", + "publish_image_with_segment_for_debug", + "max_range", + "image_size" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/segment_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json b/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json new file mode 100644 index 0000000000000..d166d85024903 --- /dev/null +++ b/localization/yabloc/yabloc_image_processing/schema/undistort.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for undistort", + "type": "object", + "definitions": { + "undistort": { + "type": "object", + "properties": { + "use_sensor_qos": { + "type": "boolean", + "description": "whether to use sensor qos or not", + "default": true + }, + "width": { + "type": "number", + "description": "resized image width size", + "default": 800 + }, + "override_frame_id": { + "type": "string", + "description": "value for overriding the camera's frame_id. if blank, frame_id of static_tf is not overwritten", + "default": "" + } + }, + "required": ["use_sensor_qos", "width", "override_frame_id"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/undistort" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_monitor/README.md b/localization/yabloc/yabloc_monitor/README.md index ed4cdc36b6ba0..849429290b427 100644 --- a/localization/yabloc/yabloc_monitor/README.md +++ b/localization/yabloc/yabloc_monitor/README.md @@ -25,3 +25,7 @@ To be added, | Name | Type | Description | | -------------- | --------------------------------- | ------------------- | | `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +### Parameters + +{{ json_to_markdown("localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json") }} diff --git a/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json b/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json new file mode 100644 index 0000000000000..c83a9d3ac45f3 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/schema/yabloc_monitor.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for yabloc_monitor", + "type": "object", + "definitions": { + "yabloc_monitor": { + "type": "object", + "properties": { + "availability/timestamp_tolerance": { + "type": "number", + "description": "tolerable time difference between current time and latest estimated pose", + "default": 1.0 + } + }, + "required": ["availability/timestamp_tolerance"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/yabloc_monitor" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md index d42ce4647bc95..f46a363102c8b 100644 --- a/localization/yabloc/yabloc_particle_filter/README.md +++ b/localization/yabloc/yabloc_particle_filter/README.md @@ -36,15 +36,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| ----------------------------- | ---------------- | ----------------------------------------------------------------- | -| `visualize` | bool | whether particles are also published in visualization_msgs or not | -| `static_linear_covariance` | double | to override the covariance of `/twist_with_covariance` | -| `static_angular_covariance` | double | to override the covariance of `/twist_with_covariance` | -| `resampling_interval_seconds` | double | the interval of particle resampling | -| `num_of_particles` | int | the number of particles | -| `prediction_rate` | double | frequency of forecast updates, in Hz | -| `cov_xx_yy` | vector\ | the covariance of initial pose | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json") }} ### Services @@ -80,19 +72,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| -------------------------------- | ------ | ---------------------------------------------------------------------------------------- | -| `acceptable_max_delay` | double | how long to hold the predicted particles | -| `visualize` | double | whether publish particles as marker_array or not | -| `mahalanobis_distance_threshold` | double | if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips. | -| `for_fixed/max_weight` | bool | parameter for gnss weight distribution | -| `for_fixed/flat_radius` | bool | parameter for gnss weight distribution | -| `for_fixed/max_radius` | bool | parameter for gnss weight distribution | -| `for_fixed/min_weight` | bool | parameter for gnss weight distribution | -| `for_not_fixed/flat_radius` | bool | parameter for gnss weight distribution | -| `for_not_fixed/max_radius` | bool | parameter for gnss weight distribution | -| `for_not_fixed/min_weight` | bool | parameter for gnss weight distribution | -| `for_not_fixed/max_weight` | bool | parameter for gnss weight distribution | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json") }} ## camera_particle_corrector @@ -127,16 +107,7 @@ This package contains some executable nodes related to particle filter. ### Parameters -| Name | Type | Description | -| ---------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------- | -| `acceptable_max_delay` | double | how long to hold the predicted particles | -| `visualize` | double | whether publish particles as marker_array or not | -| `image_size` | int | image size of debug/cost_map_image | -| `max_range` | double | width of hierarchical cost map | -| `gamma` | double | gamma value of the intensity gradient of the cost map | -| `min_prob` | double | minimum particle weight the corrector node gives | -| `far_weight_gain` | double | `exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important | -| `enabled_at_first` | bool | if it is false, this node is not activated at first. you can activate by service call | +{{ json_to_markdown("localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json") }} ### Services diff --git a/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json b/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json new file mode 100644 index 0000000000000..a8b4873347f52 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/camera_particle_corrector.schema.json @@ -0,0 +1,77 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for camera_particle_corrector", + "type": "object", + "definitions": { + "camera_particle_corrector": { + "type": "object", + "properties": { + "acceptable_max_delay": { + "type": "number", + "description": "how long to hold the predicted particles", + "default": 1.0 + }, + "visualize": { + "type": "boolean", + "description": "whether publish particles as marker_array or not", + "default": false + }, + "image_size": { + "type": "number", + "description": "image size of debug/cost_map_image", + "default": 800 + }, + "max_range": { + "type": "number", + "description": "width of hierarchical cost map", + "default": 40.0 + }, + "gamma": { + "type": "number", + "description": "gamma value of the intensity gradient of the cost map", + "default": 5.0 + }, + "min_prob": { + "type": "number", + "description": "minimum particle weight the corrector node gives", + "default": 0.1 + }, + "far_weight_gain": { + "type": "number", + "description": "`exp(-far_weight_gain_ * squared_distance_from_camera)` is weight gain. if this is large, the nearby road markings will be more important", + "default": 0.001 + }, + "enabled_at_first": { + "type": "boolean", + "description": "if it is false, this node is not activated at first. you can activate by service call", + "default": true + } + }, + "required": [ + "acceptable_max_delay", + "visualize", + "image_size", + "max_range", + "gamma", + "min_prob", + "far_weight_gain", + "enabled_at_first" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/camera_particle_corrector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json b/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json new file mode 100644 index 0000000000000..a3990ad333981 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/gnss_particle_corrector.schema.json @@ -0,0 +1,94 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for gnss_particle_corrector", + "type": "object", + "definitions": { + "gnss_particle_corrector": { + "type": "object", + "properties": { + "acceptable_max_delay": { + "type": "number", + "description": "how long to hold the predicted particles", + "default": 1.0 + }, + "visualize": { + "type": "boolean", + "description": "whether publish particles as marker_array or not", + "default": false + }, + "mahalanobis_distance_threshold": { + "type": "number", + "description": "if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips.", + "default": 30.0 + }, + "for_fixed/max_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 5.0 + }, + "for_fixed/flat_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 0.5 + }, + "for_fixed/max_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 10.0 + }, + "for_fixed/min_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is fixed", + "default": 0.5 + }, + "for_not_fixed/max_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 1.0 + }, + "for_not_fixed/flat_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 5.0 + }, + "for_not_fixed/max_radius": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 20.0 + }, + "for_not_fixed/min_weight": { + "type": "number", + "description": "gnss weight distribution used when observation is not fixed", + "default": 0.5 + } + }, + "required": [ + "acceptable_max_delay", + "visualize", + "for_fixed/max_weight", + "for_fixed/flat_radius", + "for_fixed/max_radius", + "for_fixed/min_weight", + "for_not_fixed/max_weight", + "for_not_fixed/flat_radius", + "for_not_fixed/max_radius", + "for_not_fixed/min_weight" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/gnss_particle_corrector" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json b/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json new file mode 100644 index 0000000000000..487dffdb00094 --- /dev/null +++ b/localization/yabloc/yabloc_particle_filter/schema/predictor.schema.json @@ -0,0 +1,71 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for predictor", + "type": "object", + "definitions": { + "predictor": { + "type": "object", + "properties": { + "visualize": { + "type": "boolean", + "description": "whether particles are also published in visualization_msgs or not", + "default": true + }, + "static_linear_covariance": { + "type": "number", + "description": "overriding covariance of `/twist_with_covariance`", + "default": 0.04 + }, + "static_angular_covariance": { + "type": "number", + "description": "overriding covariance of `/twist_with_covariance`", + "default": 0.006 + }, + "resampling_interval_seconds": { + "type": "number", + "description": "the interval of particle resampling", + "default": 1.0 + }, + "num_of_particles": { + "type": "number", + "description": "the number of particles", + "default": 500 + }, + "prediction_rate": { + "type": "number", + "description": "frequency of forecast updates, in Hz", + "default": 50.0 + }, + "cov_xx_yy": { + "type": "array", + "description": "the covariance of initial pose", + "default": [2.0, 0.25] + } + }, + "required": [ + "visualize", + "static_linear_covariance", + "static_angular_covariance", + "resampling_interval_seconds", + "num_of_particles", + "prediction_rate", + "cov_xx_yy" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/predictor" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index 164f280becae8..91b272b413c4c 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(PCL REQUIRED COMPONENTS common kdtree) # Sophus find_package(Sophus REQUIRED) +# OpenCV +find_package(OpenCV REQUIRED) + # =================================================== # Executable # Camera @@ -28,6 +31,7 @@ ament_auto_add_executable(${TARGET} target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) +ament_target_dependencies(${TARGET} OpenCV) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 907b79c1459ec..33b9788cca3d3 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -59,9 +59,7 @@ Converted model URL ### Parameters -| Name | Type | Description | -| ------------------ | ---- | ----------------------------------------- | -| `angle_resolution` | int | how many divisions of 1 sigma angle range | +{{ json_to_markdown("localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json") }} ### Services diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index e9921db50093e..afd0d4aa86bcf 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -17,8 +17,10 @@ rosidl_default_generators autoware_auto_mapping_msgs + cv_bridge geometry_msgs lanelet2_extension + libopencv-dev rclcpp sensor_msgs tier4_localization_msgs diff --git a/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json b/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json new file mode 100644 index 0000000000000..26a53e0fe408f --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/schema/camera_pose_initializer.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for camera_pose_initializer", + "type": "object", + "definitions": { + "camera_pose_initializer": { + "type": "object", + "properties": { + "angle_resolution": { + "type": "number", + "description": "how many divisions of 1 sigma angle range", + "default": 30 + } + }, + "required": ["angle_resolution"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/camera_pose_initializer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_height_fitter/CMakeLists.txt b/map/map_height_fitter/CMakeLists.txt index fb5b3feac6418..2f123d40c417e 100644 --- a/map/map_height_fitter/CMakeLists.txt +++ b/map/map_height_fitter/CMakeLists.txt @@ -14,4 +14,7 @@ ament_auto_add_executable(node src/node.cpp ) -ament_auto_package(INSTALL_TO_SHARE launch) +ament_auto_package( + INSTALL_TO_SHARE + launch + config) diff --git a/map/map_height_fitter/README.md b/map/map_height_fitter/README.md index 9b768f87c432b..7da70a0ff7766 100644 --- a/map/map_height_fitter/README.md +++ b/map/map_height_fitter/README.md @@ -4,8 +4,18 @@ This library fits the given point with the ground of the point cloud map. The map loading operation is switched by the parameter `enable_partial_load` of the node specified by `map_loader_name`. The node using this library must use multi thread executor. -| Interface | Local Name | Description | -| ------------ | ------------------ | ---------------------------------------- | -| Parameter | map_loader_name | The point cloud map loader name. | -| Subscription | ~/pointcloud_map | The topic name to load the whole map | -| Client | ~/partial_map_load | The service name to load the partial map | +## Parameters + +{{ json_to_markdown("map/map_height_fitter/schema/map_height_fitter.schema.json") }} + +## Topic subscription + +| Topic Name | Description | +| ---------------- | -------------------------------------------------------------------------------------------- | +| ~/pointcloud_map | The topic containing the whole pointcloud map (only used when `enable_partial_load = false`) | + +## Service client + +| Service Name | Description | +| ------------------ | ----------------------------------- | +| ~/partial_map_load | The service to load the partial map | diff --git a/map/map_height_fitter/config/map_height_fitter.param.yaml b/map/map_height_fitter/config/map_height_fitter.param.yaml new file mode 100644 index 0000000000000..45d51a7efb83f --- /dev/null +++ b/map/map_height_fitter/config/map_height_fitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + map_loader_name: "/map/pointcloud_map_loader" diff --git a/map/map_height_fitter/launch/map_height_fitter.launch.xml b/map/map_height_fitter/launch/map_height_fitter.launch.xml index 2edbdb831590d..955bb66cf6e8d 100644 --- a/map/map_height_fitter/launch/map_height_fitter.launch.xml +++ b/map/map_height_fitter/launch/map_height_fitter.launch.xml @@ -1,8 +1,10 @@ + + - + diff --git a/map/map_height_fitter/schema/map_height_fitter.schema.json b/map/map_height_fitter/schema/map_height_fitter.schema.json new file mode 100644 index 0000000000000..69798d92d2fe6 --- /dev/null +++ b/map/map_height_fitter/schema/map_height_fitter.schema.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for map_height_fitter", + "type": "object", + "definitions": { + "map_height_fitter": { + "type": "object", + "properties": { + "map_loader_name": { + "type": "string", + "description": "Node name of the map loader from which this map_height_fitter will retrieve its parameters", + "default": "/map/pointcloud_map_loader" + } + }, + "required": ["map_loader_name"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_height_fitter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 0103c7d2b74a7..a9b657f843447 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -5,6 +5,7 @@ 0.1.0 The map_loader package Ryohsuke Mitsudome + Yamato Ando Ryu Yamamoto Koji Minoda Masahiro Sakamoto diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 5db00fe0ad2f5..03504cc8e2e20 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -74,6 +74,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( // load map from file const auto map = load_map(lanelet2_filename, *msg); if (!map) { + RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published."); return; } @@ -87,6 +88,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( pub_map_bin_ = create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); + RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( @@ -105,6 +107,12 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( const lanelet::projection::LocalProjector projector; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (!errors.empty()) { + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + } + // overwrite local_x, local_y for (lanelet::Point3d point : map->pointLayer) { if (point.hasAttribute("local_x")) { diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 700d468ed4431..f6102a1efa795 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -59,4 +59,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 94b142da3dcf9..848fcfba95f14 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -28,6 +28,15 @@ sample-map-rosbag projector_type: local ``` +#### Limitation + +The functionality that requires latitude and longitude will become unavailable. + +The currently identified unavailable functionalities are: + +- GNSS localization +- Sending the self-position in latitude and longitude using ADAPI + ### Using MGRS If you want to use MGRS, please specify the MGRS grid as well. @@ -39,6 +48,10 @@ vertical_datum: WGS84 mgrs_grid: 54SUE ``` +#### Limitation + +It cannot be used with maps that span across two or more MGRS grids. Please use it only when it falls within the scope of a single MGRS grid. + ### Using LocalCartesianUTM If you want to use local cartesian UTM, please specify the map origin as well. @@ -69,11 +82,10 @@ map_origin: ## Published Topics -- ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information +- `~/map_projector_info` (tier4_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information ## Parameters -| Name | Type | Description | -| :---------------------- | :---------- | :------------------------------------------------------------------------------- | -| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) | -| lanelet2_map_path | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) | +Note that these parameters are assumed to be passed from launch arguments, and it is not recommended to directly write them in `map_projection_loader.param.yaml`. + +{{ json_to_markdown("map/map_projection_loader/schema/map_projection_loader.schema.json") }} diff --git a/map/map_projection_loader/config/map_projection_loader.param.yaml b/map/map_projection_loader/config/map_projection_loader.param.yaml new file mode 100644 index 0000000000000..6ec300309a308 --- /dev/null +++ b/map/map_projection_loader/config/map_projection_loader.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + map_projector_info_path: $(var map_projector_info_path) + lanelet2_map_path: $(var lanelet2_map_path) diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index fc625e3162911..a6570b69d3498 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -1,9 +1,10 @@ + + - - + diff --git a/map/map_projection_loader/schema/map_projection_loader.schema.json b/map/map_projection_loader/schema/map_projection_loader.schema.json new file mode 100644 index 0000000000000..bb7fe5d2910ad --- /dev/null +++ b/map/map_projection_loader/schema/map_projection_loader.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for map_projection_loader", + "type": "object", + "definitions": { + "map_projection_loader": { + "type": "object", + "properties": { + "map_projector_info_path": { + "type": "string", + "description": "The path where map_projector_info.yaml is located", + "default": "$(var map_projector_info_path)" + }, + "lanelet2_map_path": { + "type": "string", + "description": "The path where the lanelet2 map file (.osm) is located", + "default": "$(var lanelet2_map_path)" + } + }, + "required": ["map_projector_info_path", "lanelet2_map_path"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_projection_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_tf_generator/CMakeLists.txt b/map/map_tf_generator/CMakeLists.txt index 4e6390e0cacb6..e20289766cdda 100644 --- a/map/map_tf_generator/CMakeLists.txt +++ b/map/map_tf_generator/CMakeLists.txt @@ -48,5 +48,6 @@ if(BUILD_TESTING) endif() ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/map/map_tf_generator/Readme.md b/map/map_tf_generator/README.md similarity index 83% rename from map/map_tf_generator/Readme.md rename to map/map_tf_generator/README.md index 1b858ec12c2c2..643f4233c06f0 100644 --- a/map/map_tf_generator/Readme.md +++ b/map/map_tf_generator/README.md @@ -43,10 +43,7 @@ None ### Core Parameters -| Name | Type | Default Value | Explanation | -| -------------- | ------ | ------------- | ------------------------------------- | -| `viewer_frame` | string | viewer | Name of `viewer` frame | -| `map_frame` | string | map | The parent frame name of viewer frame | +{{ json_to_markdown("map/map_tf_generator/schema/map_tf_generator.schema.json") }} ## Assumptions / Known limits diff --git a/map/map_tf_generator/config/map_tf_generator.param.yaml b/map/map_tf_generator/config/map_tf_generator.param.yaml new file mode 100644 index 0000000000000..90790808acace --- /dev/null +++ b/map/map_tf_generator/config/map_tf_generator.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + map_frame: map + viewer_frame: viewer diff --git a/map/map_tf_generator/launch/map_tf_generator.launch.xml b/map/map_tf_generator/launch/map_tf_generator.launch.xml index ab5b515f8e61d..43d487c2bc982 100644 --- a/map/map_tf_generator/launch/map_tf_generator.launch.xml +++ b/map/map_tf_generator/launch/map_tf_generator.launch.xml @@ -1,13 +1,11 @@ - + - - + - - + diff --git a/map/map_tf_generator/schema/map_tf_generator.schema.json b/map/map_tf_generator/schema/map_tf_generator.schema.json new file mode 100644 index 0000000000000..e501f7a9678f6 --- /dev/null +++ b/map/map_tf_generator/schema/map_tf_generator.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Map Tf Generator", + "type": "object", + "definitions": { + "map_tf_generator": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The parent frame name of viewer frame", + "default": "map" + }, + "viewer_frame": { + "type": "string", + "description": "Name of `viewer` frame", + "default": "viewer" + } + }, + "required": ["map_frame", "viewer_frame"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_tf_generator" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp index bc686b17fbcab..69a66cf52c865 100644 --- a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -36,8 +36,8 @@ class PcdMapTFGeneratorNode : public rclcpp::Node explicit PcdMapTFGeneratorNode(const rclcpp::NodeOptions & options) : Node("pcd_map_tf_generator", options) { - map_frame_ = declare_parameter("map_frame", "map"); - viewer_frame_ = declare_parameter("viewer_frame", "viewer"); + map_frame_ = declare_parameter("map_frame"); + viewer_frame_ = declare_parameter("viewer_frame"); sub_ = create_subscription( "pointcloud_map", rclcpp::QoS{1}.transient_local(), diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index 055d71cfb2b35..20ca1c037a578 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -31,8 +31,8 @@ class VectorMapTFGeneratorNode : public rclcpp::Node explicit VectorMapTFGeneratorNode(const rclcpp::NodeOptions & options) : Node("vector_map_tf_generator", options) { - map_frame_ = declare_parameter("map_frame", "map"); - viewer_frame_ = declare_parameter("viewer_frame", "viewer"); + map_frame_ = declare_parameter("map_frame"); + viewer_frame_ = declare_parameter("viewer_frame"); sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), diff --git a/mkdocs_macros.py b/mkdocs_macros.py index 56292a815d80d..97f76442be491 100644 --- a/mkdocs_macros.py +++ b/mkdocs_macros.py @@ -42,6 +42,8 @@ def format_param_range(param): def extract_parameter_info(parameters, namespace=""): params = [] for k, v in parameters.items(): + if "$ref" in v.keys(): + continue if v["type"] != "object": param = {} param["Name"] = namespace + k diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt index 49506f4b439fb..5ad0de572b44f 100644 --- a/perception/cluster_merger/CMakeLists.txt +++ b/perception/cluster_merger/CMakeLists.txt @@ -24,4 +24,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/cluster_merger/config/cluster_merger.param.yaml b/perception/cluster_merger/config/cluster_merger.param.yaml new file mode 100644 index 0000000000000..adca6c203282f --- /dev/null +++ b/perception/cluster_merger/config/cluster_merger.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + output_frame_id: "base_link" diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml index 1bbd0ebd91e12..f0f90efe051a0 100644 --- a/perception/cluster_merger/launch/cluster_merger.launch.xml +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -3,12 +3,12 @@ - + - + diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index f7a27a52bfa8a..cddee782e8af0 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -13,3 +13,4 @@ [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] using_2d_validator: false + enable_debugger: false diff --git a/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml new file mode 100644 index 0000000000000..fc5c634735e23 --- /dev/null +++ b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + enable_debug: false diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 799b605658345..a1d941e66db4b 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -9,7 +9,6 @@ - diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 04bcbd87172b3..3acb1f2d1907a 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,6 @@ - + diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index 66c25396a656e..b8d4f61a9cf28 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -76,6 +76,13 @@ def load_composable_node_param(param_path): executable="component_container", composable_node_descriptions=[], output="screen", + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + target_container = ( + LaunchConfiguration("pointcloud_container_name") + if IfCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) + else container ) use_low_height_pointcloud_loader = LoadComposableNodes( @@ -83,13 +90,13 @@ def load_composable_node_param(param_path): low_height_cropbox_filter_component, use_low_height_euclidean_component, ], - target_container=container, + target_container=target_container, condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) disuse_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[disuse_low_height_euclidean_component], - target_container=container, + target_container=target_container, condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) @@ -106,6 +113,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_low_height_cropbox", "false"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index fd1ea82befae0..f4deeccf7b76c 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -5,6 +5,8 @@ + + @@ -12,5 +14,8 @@ + + + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 00bcd4422bd3c..607e1bf30ccaa 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -77,6 +77,13 @@ def load_composable_node_param(param_path): executable="component_container", composable_node_descriptions=[], output="screen", + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + target_container = ( + LaunchConfiguration("pointcloud_container_name") + if IfCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) + else container ) use_low_height_pointcloud_loader = LoadComposableNodes( @@ -84,13 +91,13 @@ def load_composable_node_param(param_path): low_height_cropbox_filter_component, use_low_height_euclidean_component, ], - target_container=container, + target_container=target_container, condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) disuse_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[disuse_low_height_euclidean_component], - target_container=container, + target_container=target_container, condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ @@ -110,6 +117,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_low_height_cropbox", "false"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index b6a426dabfd12..3a7c685d8f449 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -5,6 +5,8 @@ + + @@ -12,5 +14,8 @@ + + + diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index bdd793896062a..656ffec486c3c 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -89,6 +89,7 @@ if(BUILD_TESTING) target_link_libraries(test_ray_ground_filter ground_segmentation + ${YAML_CPP_LIBRARIES} ) ament_add_ros_isolated_gtest(test_scan_ground_filter diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 8d56e12244716..756882391183e 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/perception/ground_segmentation/config/ground_segmentation.param.yaml @@ -29,3 +29,7 @@ gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml new file mode 100644 index 0000000000000..0d4a7d574499c --- /dev/null +++ b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + base_frame: "base_link" + unit_axis: "z" + max_iterations: 1000 + min_trial: 5000 + min_points: 1000 + outlier_threshold: 0.01 + plane_slope_threshold: 10.0 + voxel_size_x: 0.04 + voxel_size_y: 0.04 + voxel_size_z: 0.04 + height_threshold: 0.01 + debug: false diff --git a/perception/ground_segmentation/config/ray_ground_filter.param.yaml b/perception/ground_segmentation/config/ray_ground_filter.param.yaml new file mode 100644 index 0000000000000..5b9e8c06595f7 --- /dev/null +++ b/perception/ground_segmentation/config/ray_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + min_x: -0.01 + max_x: 0.01 + min_y: -0.01 + max_y: 0.01 + use_vehicle_footprint: false + general_max_slope: 8.0 + local_max_slope: 6.0 + initial_max_slope: 3.0 + radial_divider_angle: 1.0 + min_height_threshold: 0.15 + concentric_divider_distance: 0.0 + reclass_distance_threshold: 0.1 diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml new file mode 100644 index 0000000000000..bc02a1ab7e6e7 --- /dev/null +++ b/perception/ground_segmentation/config/scan_ground_filter.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.2 + use_virtual_ground_point: True + split_height_distance: 0.2 + non_ground_height_threshold: 0.20 + grid_size_m: 0.1 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 + elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml new file mode 100644 index 0000000000000..f8280b774c6b0 --- /dev/null +++ b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml new file mode 100644 index 0000000000000..86375c520a426 --- /dev/null +++ b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml new file mode 100644 index 0000000000000..2a9e4983ecb56 --- /dev/null +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index ebcb02df2c614..ed5fb6abe7fe7 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param; RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RANSACGroundFilter", options) { - base_frame_ = declare_parameter("base_frame", "base_link"); - unit_axis_ = declare_parameter("unit_axis", "z"); - max_iterations_ = declare_parameter("max_iterations", 1000); - min_inliers_ = declare_parameter("min_trial", 5000); - min_points_ = declare_parameter("min_points", 1000); - outlier_threshold_ = declare_parameter("outlier_threshold", 0.01); - plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0); - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); - voxel_size_z_ = declare_parameter("voxel_size_z", 0.04); - height_threshold_ = declare_parameter("height_threshold", 0.01); - debug_ = declare_parameter("debug", false); + base_frame_ = declare_parameter("base_frame", "base_link"); + unit_axis_ = declare_parameter("unit_axis"); + max_iterations_ = declare_parameter("max_iterations"); + min_inliers_ = declare_parameter("min_trial"); + min_points_ = declare_parameter("min_points"); + outlier_threshold_ = declare_parameter("outlier_threshold"); + plane_slope_threshold_ = declare_parameter("plane_slope_threshold"); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + height_threshold_ = declare_parameter("height_threshold"); + debug_ = declare_parameter("debug"); if (unit_axis_ == "x") { unit_vec_ = Eigen::Vector3d::UnitX(); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index 4b2e81d272e30..7bcae47fd9f1f 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o grid_precision_ = 0.2; ray_ground_filter::generateColors(colors_, color_num_); - min_x_ = declare_parameter("min_x", -0.01); - max_x_ = declare_parameter("max_x", 0.01); - min_y_ = declare_parameter("min_y", -0.01); - max_y_ = declare_parameter("max_y", 0.01); + min_x_ = declare_parameter("min_x"); + max_x_ = declare_parameter("max_x"); + min_y_ = declare_parameter("min_y"); + max_y_ = declare_parameter("max_y"); setVehicleFootprint(min_x_, max_x_, min_y_, max_y_); - use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); + use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); - general_max_slope_ = declare_parameter("general_max_slope", 8.0); - local_max_slope_ = declare_parameter("local_max_slope", 6.0); - initial_max_slope_ = declare_parameter("initial_max_slope", 3.0); - radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0); - min_height_threshold_ = declare_parameter("min_height_threshold", 0.15); - concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0); - reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1); + general_max_slope_ = declare_parameter("general_max_slope"); + local_max_slope_ = declare_parameter("local_max_slope"); + initial_max_slope_ = declare_parameter("initial_max_slope"); + radial_divider_angle_ = declare_parameter("radial_divider_angle"); + min_height_threshold_ = declare_parameter("min_height_threshold"); + concentric_divider_distance_ = declare_parameter("concentric_divider_distance"); + reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold"); } using std::placeholders::_1; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 953a9feb4d21e..f164e0102e0e9 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -37,24 +37,22 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { // set initial parameters { - low_priority_region_x_ = static_cast(declare_parameter("low_priority_region_x", -20.0f)); - detection_range_z_max_ = static_cast(declare_parameter("detection_range_z_max", 2.5f)); - center_pcl_shift_ = static_cast(declare_parameter("center_pcl_shift", 0.0)); - non_ground_height_threshold_ = - static_cast(declare_parameter("non_ground_height_threshold", 0.20)); - grid_mode_switch_radius_ = - static_cast(declare_parameter("grid_mode_switch_radius", 20.0)); - - grid_size_m_ = static_cast(declare_parameter("grid_size_m", 0.5)); - gnd_grid_buffer_size_ = static_cast(declare_parameter("gnd_grid_buffer_size", 4)); - elevation_grid_mode_ = static_cast(declare_parameter("elevation_grid_mode", true)); - global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); - local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 10.0)); - radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); - split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2); - split_height_distance_ = declare_parameter("split_height_distance", 0.2); - use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true); - use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true); + low_priority_region_x_ = declare_parameter("low_priority_region_x"); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + + grid_size_m_ = declare_parameter("grid_size_m"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); + local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_height_distance_ = declare_parameter("split_height_distance"); + use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/ground_segmentation/test/test_ray_ground_filter.cpp index af8bb29ab12b7..0c1db47a5ae09 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_ray_ground_filter.cpp @@ -29,7 +29,7 @@ #include #include #endif - +#include class RayGroundFilterComponentTestSuite : public ::testing::Test { protected: @@ -63,8 +63,25 @@ class RayGroundFilterComponentTest : public ground_segmentation::RayGroundFilter TEST_F(RayGroundFilterComponentTestSuite, TestCase1) { - // read pcd to pointcloud const auto share_dir = ament_index_cpp::get_package_share_directory("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); + auto params = config["/**"]["ros__parameters"]; + + double min_x_ = params["min_x"].as(); + double max_x_ = params["max_x"].as(); + double min_y_ = params["min_y"].as(); + double max_y_ = params["max_y"].as(); + bool use_vehicle_footprint_ = params["use_vehicle_footprint"].as(); + double general_max_slope_ = params["general_max_slope"].as(); + double local_max_slope_ = params["local_max_slope"].as(); + double initial_max_slope_ = params["initial_max_slope"].as(); + double radial_divider_angle_ = params["radial_divider_angle"].as(); + double min_height_threshold_ = params["min_height_threshold"].as(); + double concentric_divider_distance_ = params["concentric_divider_distance"].as(); + double reclass_distance_threshold_ = params["reclass_distance_threshold"].as(); + const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; pcl::io::loadPCDFile(pcd_path, cloud); @@ -94,10 +111,23 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) rclcpp::NodeOptions node_options; std::vector parameters; + parameters.emplace_back(rclcpp::Parameter("base_frame", "base_link")); - parameters.emplace_back(rclcpp::Parameter("general_max_slope", 2.0)); - parameters.emplace_back(rclcpp::Parameter("local_max_slope", 3.0)); - parameters.emplace_back(rclcpp::Parameter("initial_max_slope", 1.0)); + parameters.emplace_back(rclcpp::Parameter("general_max_slope", general_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("local_max_slope", local_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("initial_max_slope", initial_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("radial_divider_angle", radial_divider_angle_)); + parameters.emplace_back(rclcpp::Parameter("min_height_threshold", min_height_threshold_)); + parameters.emplace_back( + rclcpp::Parameter("concentric_divider_distance", concentric_divider_distance_)); + parameters.emplace_back( + rclcpp::Parameter("reclass_distance_threshold", reclass_distance_threshold_)); + parameters.emplace_back(rclcpp::Parameter("min_x", min_x_)); + parameters.emplace_back(rclcpp::Parameter("max_x", max_x_)); + parameters.emplace_back(rclcpp::Parameter("min_y", min_y_)); + parameters.emplace_back(rclcpp::Parameter("max_y", max_y_)); + parameters.emplace_back(rclcpp::Parameter("use_vehicle_footprint", use_vehicle_footprint_)); + node_options.parameter_overrides(parameters); auto ray_ground_filter_test = std::make_shared(node_options); ray_ground_filter_test->input_pointcloud_pub_->publish(*input_msg_ptr); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index acc81f8817cd8..9df9c7e9c7433 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -38,6 +38,8 @@ class ScanGroundFilterTest : public ::testing::Test { rclcpp::init(0, nullptr); + parse_yaml(); + dummy_node_ = std::make_shared("ScanGroundFilterTest"); input_pointcloud_pub_ = rclcpp::create_publisher( dummy_node_, "/test_scan_ground_filter/input_cloud", 1); @@ -58,6 +60,30 @@ class ScanGroundFilterTest : public ::testing::Test parameters.emplace_back(rclcpp::Parameter("right_overhang", 0.1)); parameters.emplace_back(rclcpp::Parameter("vehicle_height", 2.5)); parameters.emplace_back(rclcpp::Parameter("max_steer_angle", 0.7)); + + parameters.emplace_back( + rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); + parameters.emplace_back( + rclcpp::Parameter("use_virtual_ground_point", use_virtual_ground_point_)); + parameters.emplace_back(rclcpp::Parameter("split_height_distance", split_height_distance_)); + parameters.emplace_back( + rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); + parameters.emplace_back(rclcpp::Parameter("grid_size_m", grid_size_m_)); + parameters.emplace_back(rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); + parameters.emplace_back(rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); + parameters.emplace_back(rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); + parameters.emplace_back(rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); + parameters.emplace_back(rclcpp::Parameter("low_priority_region_x", low_priority_region_x_)); + parameters.emplace_back(rclcpp::Parameter("center_pcl_shift", center_pcl_shift_)); + parameters.emplace_back( + rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); + options.parameter_overrides(parameters); scan_ground_filter_ = std::make_shared(options); @@ -88,8 +114,6 @@ class ScanGroundFilterTest : public ::testing::Test t.transform.rotation.w = q.w(); tf2::doTransform(*origin_input_msg_ptr, *input_msg_ptr_, t); - - parse_yaml(); } ScanGroundFilterTest() {} @@ -113,10 +137,10 @@ class ScanGroundFilterTest : public ::testing::Test void parse_yaml() { const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); - const auto config_path = share_dir + "/config/ground_segmentation.param.yaml"; + 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); - auto params = config["/**"]["ros__parameters"]["common_ground_filter"]["parameters"]; + auto params = config["/**"]["ros__parameters"]; global_slope_max_angle_deg_ = params["global_slope_max_angle_deg"].as(); local_slope_max_angle_deg_ = params["local_slope_max_angle_deg"].as(); split_points_distance_tolerance_ = params["split_points_distance_tolerance"].as(); @@ -127,6 +151,11 @@ class ScanGroundFilterTest : public ::testing::Test gnd_grid_buffer_size_ = params["gnd_grid_buffer_size"].as(); detection_range_z_max_ = params["detection_range_z_max"].as(); elevation_grid_mode_ = params["elevation_grid_mode"].as(); + low_priority_region_x_ = params["low_priority_region_x"].as(); + use_virtual_ground_point_ = params["use_virtual_ground_point"].as(); + center_pcl_shift_ = params["center_pcl_shift"].as(); + radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); + use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -139,6 +168,11 @@ class ScanGroundFilterTest : public ::testing::Test uint16_t gnd_grid_buffer_size_ = 0; float detection_range_z_max_ = 0.0; bool elevation_grid_mode_ = false; + float low_priority_region_x_ = 0.0; + bool use_virtual_ground_point_; + float center_pcl_shift_; + float radial_divider_angle_deg_; + bool use_recheck_ground_cluster_; }; TEST_F(ScanGroundFilterTest, TestCase1) @@ -146,27 +180,6 @@ TEST_F(ScanGroundFilterTest, TestCase1) input_pointcloud_pub_->publish(*input_msg_ptr_); sensor_msgs::msg::PointCloud2 out_cloud; - // set filter parameter - scan_ground_filter_->set_parameter( - rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_height_distance", split_height_distance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); - scan_ground_filter_->set_parameter(rclcpp::Parameter("grid_size_m", grid_size_m_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); - filter(out_cloud); output_pointcloud_pub_->publish(out_cloud); diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index dba64a27232b1..e83cf92400a60 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -53,6 +53,15 @@ The timeout threshold should be set according to the postprocessing time. E.g, if the postprocessing time is around 50ms, the timeout threshold should be set smaller than 50ms, so that the whole processing time could be less than 100ms. current default value at autoware.universe for XX1: - timeout_ms: 50.0 +#### The `build_only` option + +The `pointpainting_fusion` 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 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 +``` + #### Known Limits The rclcpp::TimerBase timer could not break a for loop, therefore even if time is out when fusing a roi msg at the middle, the program will run until all msgs are fused. diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 21d31f216373b..8ccd46978630d 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,5 +1,11 @@ /**: ros__parameters: + trt_precision: fp16 + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" + model_params: class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 914ad13519807..79a8b860ebdd3 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -28,3 +28,13 @@ # this is selected based on semantic segmentation model accuracy, # calibration accuracy and unknown reaction distance filter_distance_threshold: 60.0 + + # debug + debug_mode: false + filter_scope_min_x: -100 + filter_scope_max_x: 100 + filter_scope_min_y: -100 + filter_scope_max_y: 100 + filter_scope_min_z: -100 + filter_scope_max_z: 100 + image_buffer_size: 15 diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index fe8acb6746dba..814fc5eca087e 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -44,8 +44,6 @@ #include #include -// cspell: ignore minx, maxx, miny, maxy, minz, maxz - namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; @@ -131,13 +129,12 @@ class FusionNode : public rclcpp::Node // debugger std::shared_ptr debugger_; virtual bool out_of_scope(const ObjType & obj) = 0; - // cspell: ignore minx, maxx, miny, maxy, minz, maxz - float filter_scope_minx_; - float filter_scope_maxx_; - float filter_scope_miny_; - float filter_scope_maxy_; - float filter_scope_minz_; - float filter_scope_maxz_; + float filter_scope_min_x_; + float filter_scope_max_x_; + float filter_scope_min_y_; + float filter_scope_max_y_; + float filter_scope_min_z_; + float filter_scope_max_z_; /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index 33781461fa1cc..f336eb16e94a1 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -26,6 +26,9 @@ + + + @@ -38,19 +41,59 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - + + + diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 52dd71e9579c1..8df1a374b00b6 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -22,7 +22,6 @@ - diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml index 96bf47594bfe8..1db2bb20793ac 100644 --- a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -20,17 +20,6 @@ - - - - - - - - - - - @@ -72,16 +61,6 @@ - - - - - - - - - - diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 49ff4dafc7900..1322d053b358a 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -5,7 +5,6 @@ 0.1.0 The image_projection_based_fusion package Yukihiro Saito - Yusuke Muramatsu Shunsuke Miura Yoshi Ri badai nguyen diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 6bced39bc61ef..f06ff96abe257 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -137,14 +137,12 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("processing_time"); } - // cspell: ignore minx, maxx, miny, maxy, minz, maxz - // FIXME: use min_x instead of minx - filter_scope_minx_ = declare_parameter("filter_scope_min_x"); - filter_scope_maxx_ = declare_parameter("filter_scope_max_x"); - filter_scope_miny_ = declare_parameter("filter_scope_min_y"); - filter_scope_maxy_ = declare_parameter("filter_scope_max_y"); - filter_scope_minz_ = declare_parameter("filter_scope_min_z"); - filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); + filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); + filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); + filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); + filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); + filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); + filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); } template diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 1c064e4d0f060..feaf1ad495ce8 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -28,8 +28,6 @@ #include #endif -// cspell: ignore minx, maxx, miny, maxy, minz, maxz - namespace image_projection_based_fusion { @@ -253,17 +251,17 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) for (sensor_msgs::PointCloud2ConstIterator iter_x(cluster, "x"), iter_y(cluster, "y"), iter_z(cluster, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (!valid_point(*iter_x, filter_scope_minx_, filter_scope_maxx_)) { + if (!valid_point(*iter_x, filter_scope_min_x_, filter_scope_max_x_)) { is_out = true; break; } - if (!valid_point(*iter_y, filter_scope_miny_, filter_scope_maxy_)) { + if (!valid_point(*iter_y, filter_scope_min_y_, filter_scope_max_y_)) { is_out = true; break; } - if (!valid_point(*iter_z, filter_scope_minz_, filter_scope_maxz_)) { + if (!valid_point(*iter_z, filter_scope_min_z_, filter_scope_max_z_)) { is_out = true; break; } diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 19defc0a1cab0..762d3e0ee51b1 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -19,8 +19,6 @@ #include #include -// cspell: ignore minx, maxx, miny, maxy, minz, maxz - namespace image_projection_based_fusion { @@ -268,13 +266,13 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return (p > min_num) && (p < max_num); }; - if (!valid_point(pose.position.x, filter_scope_minx_, filter_scope_maxx_)) { + if (!valid_point(pose.position.x, filter_scope_min_x_, filter_scope_max_x_)) { return is_out; } - if (!valid_point(pose.position.y, filter_scope_miny_, filter_scope_maxy_)) { + if (!valid_point(pose.position.y, filter_scope_min_y_, filter_scope_max_y_)) { return is_out; } - if (!valid_point(pose.position.z, filter_scope_minz_, filter_scope_maxz_)) { + if (!valid_point(pose.position.z, filter_scope_min_z_, filter_scope_max_z_)) { return is_out; } diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index f5a15916f31a9..2a9adedad9074 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -45,6 +45,15 @@ We trained the models using . | `nms_iou_threshold` | double | - | IoU threshold for the IoU-based Non Maximum Suppression | | `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built | +### The `build_only` option + +The `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 +``` + ## Assumptions / Known limits - The `object.existence_probability` is stored the value of classification confidence of a DNN, not probability. diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml index 0b29a87965b42..a9c3174846d0d 100644 --- a/perception/lidar_centerpoint/config/centerpoint.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -13,3 +13,14 @@ iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + score_threshold: 0.35 + has_twist: false + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 8252fde8273d8..474d0e2b695ac 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -13,3 +13,14 @@ iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + score_threshold: 0.35 + has_twist: false + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" diff --git a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml index ed378ffa44a70..baea087c96bca 100644 --- a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml +++ b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml @@ -29,9 +29,9 @@ max_area_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN - 0.000, 0.000, 36.000, 0.000, inf, 0.000, 0.000, 0.000, #CAR - 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #TRUCK - 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 36.000, 0.000, 999.999, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #BUS 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index a7f181ab78ac6..5489af2c8fb60 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -7,24 +7,33 @@ - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index 2d841d22c2eb1..ad1a7ae90630c 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -14,8 +14,6 @@ #include "lidar_centerpoint/network/network_trt.hpp" -#include - namespace centerpoint { bool VoxelEncoderTRT::setProfile( @@ -65,9 +63,8 @@ bool HeadTRT::setProfile( if ( out_name == std::string("heatmap") && network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - RCLCPP_ERROR( - rclcpp::get_logger("lidar_centerpoint"), - "Expected and actual number of classes do not match"); + tensorrt_common::LOG_ERROR(logger_) + << "Expected and actual number of classes do not match" << std::endl; return false; } auto out_dims = nvinfer1::Dims4( diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 2255df99dbcf4..91fcce9a80f78 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -14,8 +14,6 @@ #include "lidar_centerpoint/network/tensorrt_wrapper.hpp" -#include - #include #include @@ -42,7 +40,7 @@ bool TensorRTWrapper::init( runtime_ = tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create runtime"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; return false; } @@ -51,7 +49,11 @@ bool TensorRTWrapper::init( if (engine_file.is_open()) { success = loadEngine(engine_path); } else { + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); success = parseONNX(onnx_path, engine_path, precision); + logger_.stop_throttle(log_thread); } success &= createContext(); @@ -61,15 +63,15 @@ bool TensorRTWrapper::init( bool TensorRTWrapper::createContext() { if (!engine_) { - RCLCPP_ERROR( - rclcpp::get_logger("lidar_centerpoint"), "Failed to create context: Engine was not created"); + tensorrt_common::LOG_ERROR(logger_) + << "Failed to create context: Engine was not created" << std::endl; return false; } context_ = tensorrt_common::TrtUniquePtr(engine_->createExecutionContext()); if (!context_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create context"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; return false; } @@ -83,14 +85,14 @@ bool TensorRTWrapper::parseONNX( auto builder = tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); if (!builder) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create builder"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; return false; } auto config = tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); if (!config) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create config"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; return false; } #if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 @@ -100,12 +102,11 @@ bool TensorRTWrapper::parseONNX( #endif if (precision == "fp16") { if (builder->platformHasFastFp16()) { - RCLCPP_INFO(rclcpp::get_logger("lidar_centerpoint"), "Using TensorRT FP16 Inference"); + tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; config->setFlag(nvinfer1::BuilderFlag::kFP16); } else { - RCLCPP_INFO( - rclcpp::get_logger("lidar_centerpoint"), - "TensorRT FP16 Inference isn't supported in this environment"); + tensorrt_common::LOG_INFO(logger_) + << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; } } @@ -114,7 +115,7 @@ bool TensorRTWrapper::parseONNX( auto network = tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); if (!network) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create network"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; return false; } @@ -123,23 +124,20 @@ bool TensorRTWrapper::parseONNX( parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); if (!setProfile(*builder, *network, *config)) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to set profile"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; return false; } - RCLCPP_INFO_STREAM( - rclcpp::get_logger("lidar_centerpoint"), - "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..."); plan_ = tensorrt_common::TrtUniquePtr( builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create serialized network"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create serialized network" << std::endl; return false; } engine_ = tensorrt_common::TrtUniquePtr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create engine"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; return false; } @@ -148,7 +146,7 @@ bool TensorRTWrapper::parseONNX( bool TensorRTWrapper::saveEngine(const std::string & engine_path) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Writing to " << engine_path); + tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; std::ofstream file(engine_path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); return true; @@ -162,7 +160,7 @@ bool TensorRTWrapper::loadEngine(const std::string & engine_path) std::string engine_str = engine_buffer.str(); engine_ = tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( reinterpret_cast(engine_str.data()), engine_str.size())); - RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Loaded engine from " << engine_path); + tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; return true; } diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index ffa22964a2df9..fee97a84be917 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -4,7 +4,6 @@ lidar_centerpoint 1.0.0 The lidar_centerpoint package - Yusuke Muramatsu Kenzo Lobos-Tsunekawa Apache License 2.0 diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index fdd64174de9be..ed1193f1c49a0 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -19,6 +19,7 @@ use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds + use_crosswalk_signal: true # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index f61dc75ffb85b..2864d1c5bf393 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +109,9 @@ using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_perception_msgs::msg::TrafficSignalElement; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; @@ -122,6 +127,7 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_traffic_signals_; // Object History std::unordered_map> objects_history_; @@ -131,6 +137,8 @@ class MapBasedPredictionNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; + std::unordered_map traffic_signal_id_map_; + // parameter update OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParam( @@ -181,11 +189,14 @@ class MapBasedPredictionNode : public rclcpp::Node double speed_limit_multiplier_; double acceleration_exponential_half_life_; + bool use_crosswalk_signal_; + // Stop watch StopWatch stop_watch_; // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); + void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); bool doesPathCrossAnyFence(const PredictedPath & predicted_path); @@ -249,6 +260,8 @@ class MapBasedPredictionNode : public rclcpp::Node const LaneletsData & lanelets_data); bool isDuplicated( const PredictedPath & predicted_path, const std::vector & predicted_paths); + std::optional getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet); + std::optional getTrafficSignalElement(const lanelet::Id & id); visualization_msgs::msg::Marker getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num); diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml index 619c0c9507e0b..7f81971b2f8d7 100644 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/map_based_prediction/launch/map_based_prediction.launch.xml @@ -3,12 +3,14 @@ + + diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index b07d9855f9821..8efea9efa1844 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -16,6 +16,7 @@ autoware_cmake autoware_auto_perception_msgs + autoware_perception_msgs interpolation lanelet2_extension libgoogle-glog-dev diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c949eae21aeff..168b874ccbcce 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -788,6 +788,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ acceleration_exponential_half_life_ = declare_parameter("acceleration_exponential_half_life"); + use_crosswalk_signal_ = declare_parameter("use_crosswalk_signal"); + path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); @@ -801,6 +803,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); + sub_traffic_signals_ = this->create_subscription( + "/traffic_signals", 1, + std::bind(&MapBasedPredictionNode::trafficSignalsCallback, this, std::placeholders::_1)); pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = @@ -872,6 +877,14 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); } +void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg) +{ + traffic_signal_id_map_.clear(); + for (const auto & signal : msg->signals) { + traffic_signal_id_map_[signal.traffic_signal_id] = signal; + } +} + void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { stop_watch_.tic(); @@ -1218,6 +1231,18 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } // try to find the edge points for all crosswalks and generate path to the crosswalk edge for (const auto & crosswalk : crosswalks_) { + const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); + if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { + const auto signal_color = [&] { + const auto elem_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value()); + return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN; + }(); + + if (signal_color == TrafficSignalElement::RED) { + continue; + } + } + const auto edge_points = getCrosswalkEdgePoints(crosswalk); const auto reachable_first = hasPotentialToReach( @@ -2211,6 +2236,38 @@ bool MapBasedPredictionNode::isDuplicated( return false; } + +std::optional MapBasedPredictionNode::getTrafficSignalId( + const lanelet::ConstLanelet & way_lanelet) +{ + const auto traffic_light_reg_elems = + way_lanelet.regulatoryElementsAs(); + if (traffic_light_reg_elems.empty()) { + return std::nullopt; + } else if (traffic_light_reg_elems.size() > 1) { + RCLCPP_ERROR( + get_logger(), + "[Map Based Prediction]: " + "Multiple regulatory elements as TrafficLight are defined to one lanelet object."); + } + return traffic_light_reg_elems.front()->id(); +} + +std::optional MapBasedPredictionNode::getTrafficSignalElement( + const lanelet::Id & id) +{ + if (traffic_signal_id_map_.count(id) != 0) { + const auto & signal_elements = traffic_signal_id_map_.at(id).elements; + if (signal_elements.size() > 1) { + RCLCPP_ERROR( + get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received."); + } else if (!signal_elements.empty()) { + return signal_elements.front(); + } + } + return std::nullopt; +} + } // namespace map_based_prediction #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ad268283d5890..ba50933eddaec 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,16 +32,18 @@ class BicycleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -51,7 +53,7 @@ class BicycleTracker : public Tracker float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 8a9b6adfc9cd5..e19b6382ad952 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -32,26 +32,28 @@ class BigVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 79e9a20a10421..05fa0a5ef01ba 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -33,28 +33,30 @@ class NormalVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; @@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); + double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index f74967943be32..f1f53c7398c0a 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -11,21 +11,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude. - state transition equation $$ -\begin{align*} -x_{k+1} &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\ -y_{k+1} &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\ -\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\ -v_{x_{k+1}} &= v_{x_k} \\ -\dot{\psi}_{k+1} &= \dot{\psi}_k \\ -\end{align*} +\begin{aligned} +x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\ +y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\ +\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\ +v_{k+1} & = v_{k} \\ +\dot\psi_{k+1} & = \dot\psi_{k} \\ +\end{aligned} $$ - jacobian $$ A = \begin{bmatrix} -1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ -0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ +1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ +0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ 0 & 0 & 1 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ @@ -40,17 +40,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe ![kinematic_bicycle_model](image/kinematic_bicycle_model.png) - **state variable** - - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ ) - - $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$ + - pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ ) + - $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$ - **Prediction Equation** - $dt$: sampling time + - $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity $$ \begin{aligned} -x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\ -y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\ -v_{k+1} & =v_{k} \\ -\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\ +x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t} + -\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t} + +\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +\psi_{k+1} & =\psi_{k} + w_k {d t} \\ +v_{k+1} & = v_{k} \\ \beta_{k+1} & =\beta_{k} \end{aligned} $$ @@ -59,9 +62,15 @@ $$ $$ \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc} -1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\ -0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\ -0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\ +1 & 0 + & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 1 + & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right] diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 5e7cadc423216..f367e19c11f2a 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position, getXYCovariance(tracked_object.kinematics.pose_with_covariance)); - if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false; + if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false; } // 2d iou gate if (passed_gate) { diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 6571e70b8c123..8133bc7dcf0a0 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -52,51 +52,57 @@ BicycleTracker::BicycleTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.6; // [m/s] - float q_stddev_y = 0.6; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] - float r_stddev_x = 0.6; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 0.8; // [m/s] - float p0_stddev_y = 0.5; // [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // initial state covariance + float p0_stddev_x = 0.8; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize X matrix + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -110,7 +116,7 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -120,10 +126,10 @@ BicycleTracker::BicycleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -137,15 +143,28 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m } bool BicycleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -154,69 +173,116 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - 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); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // 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); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -310,20 +376,20 @@ bool BicycleTracker::measureWithPose( } } - // update with ekf + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -341,14 +407,26 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - return false; + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - constexpr float gain = 0.9; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + constexpr float gain = 0.9; + bounding_box_.length = + gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; + bounding_box_.height = + gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m return true; } @@ -382,7 +460,7 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -429,23 +507,44 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 97d6d48c35d1b..b5a730e93663a 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,53 +53,59 @@ BigVehicleTracker::BigVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.5; // [m/s] - float q_stddev_y = 1.5; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] - float r_stddev_x = 1.5; // [m] - float r_stddev_y = 0.5; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float r_stddev_vx = 1.0; // [m/s] - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] + float r_stddev_vel = 1.0; // [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.5; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - // X(IDX::YAW) = object.kinematics.twist_with_covariance.twist.angular.z; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -112,7 +120,7 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -122,10 +130,10 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -142,9 +150,7 @@ BigVehicleTracker::BigVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m } bool BigVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - 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); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // 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); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -249,14 +315,14 @@ bool BigVehicleTracker::measureWithPose( float r_cov_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else if (utils::isLargeVehicleLabel(label)) { + if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; @@ -267,16 +333,16 @@ bool BigVehicleTracker::measureWithPose( if (object.kinematics.has_twist) { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf_.getX(X_t); - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose measurement + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -296,7 +362,7 @@ bool BigVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix */ + /* Set measurement matrix and noise covariance*/ Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); @@ -330,31 +396,32 @@ bool BigVehicleTracker::measureWithPose( R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - /* ekf tracks constant tracking point */ + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, slip + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -372,9 +439,8 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -389,6 +455,10 @@ bool BigVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m return true; } @@ -480,23 +550,44 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -506,7 +597,6 @@ bool BigVehicleTracker::getTrackedObject( const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); - return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index aa3f7b1c30d01..ca0a2d39c266b 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,52 +53,59 @@ NormalVehicleTracker::NormalVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.0; // object coordinate [m/s] - float q_stddev_y = 1.0; // object coordinate [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] - float r_stddev_x = 1.0; // object coordinate [m] - float r_stddev_y = 0.3; // object coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float r_stddev_vx = 1.0; // object coordinate [m/s] - float p0_stddev_x = 1.0; // object coordinate [m/s] - float p0_stddev_y = 0.3; // object coordinate [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.0; // in object coordinate [m] + float p0_stddev_y = 0.3; // in object coordinate [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -111,7 +120,7 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -121,10 +130,10 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -132,8 +141,7 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } else { // past default value // bounding_box_ = {4.0, 1.7, 2.0}; @@ -142,9 +150,7 @@ NormalVehicleTracker::NormalVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m } bool NormalVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - 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); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // 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); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -253,8 +319,8 @@ bool NormalVehicleTracker::measureWithPose( r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); r_cov_y = std::pow(r_stddev_y, 2.0); } else { @@ -262,35 +328,25 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - // extract current state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - // Decide dimension of measurement vector bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose output + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } + double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); // convert to boundingbox if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; @@ -342,30 +398,30 @@ bool NormalVehicleTracker::measureWithPose( // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - // ekf update: this tracks tracking point + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -383,9 +439,8 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -400,6 +455,11 @@ bool NormalVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m + return true; } @@ -422,12 +482,6 @@ bool NormalVehicleTracker::measure( measureWithPose(object); measureWithShape(object); - // refinement - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -441,7 +495,7 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -452,6 +506,7 @@ bool NormalVehicleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -496,23 +551,44 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -534,3 +610,22 @@ void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, self_transform); } + +double NormalVehicleTracker::getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + double measurement_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) + while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { + measurement_yaw = measurement_yaw + M_PI; + } + while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { + measurement_yaw = measurement_yaw - M_PI; + } + } + return measurement_yaw; +} diff --git a/perception/object_range_splitter/CMakeLists.txt b/perception/object_range_splitter/CMakeLists.txt index 7b27a344a5e0d..92c6c0668a085 100644 --- a/perception/object_range_splitter/CMakeLists.txt +++ b/perception/object_range_splitter/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(object_range_splitter ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_range_splitter/config/object_range_splitter.param.yaml b/perception/object_range_splitter/config/object_range_splitter.param.yaml new file mode 100644 index 0000000000000..45bde990029f3 --- /dev/null +++ b/perception/object_range_splitter/config/object_range_splitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + split_range: 30.0 diff --git a/perception/object_range_splitter/launch/object_range_splitter.launch.xml b/perception/object_range_splitter/launch/object_range_splitter.launch.xml index 6588c3e71ef73..3f2f3c6ba24c6 100644 --- a/perception/object_range_splitter/launch/object_range_splitter.launch.xml +++ b/perception/object_range_splitter/launch/object_range_splitter.launch.xml @@ -3,12 +3,12 @@ - + - + diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/object_velocity_splitter/CMakeLists.txt index bcc56f522f0a8..0bf6de39f8f3c 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/object_velocity_splitter/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml new file mode 100644 index 0000000000000..adac831aa402c --- /dev/null +++ b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_threshold: 3.0 diff --git a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml index 42d7f3d61f386..8ab654d4907a4 100644 --- a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml +++ b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml @@ -5,13 +5,13 @@ - + - + diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index 81b1daabaa7ef..6f1270c656bde 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml b/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml new file mode 100644 index 0000000000000..f52edf641fb52 --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + angle_threshold: 1.2210 + velocity_threshold: 1.5 diff --git a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml b/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml index 5c414f37a36a4..25856e12c3ba5 100644 --- a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml +++ b/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml @@ -5,15 +5,13 @@ - - + - - + diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/radar_crossing_objects_noise_filter/package.xml index eb9ea792d54bc..847ed47743c83 100644 --- a/perception/radar_crossing_objects_noise_filter/package.xml +++ b/perception/radar_crossing_objects_noise_filter/package.xml @@ -7,6 +7,8 @@ Sathshi Tanaka Shunsuke Miura Yoshi Ri + Taekjin Lee + Apache License 2.0 Sathshi Tanaka diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 21641be7b3b7c..4c09f68d88053 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -6,6 +6,9 @@ radar_fusion_to_detected_object Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Satoshi Tanaka diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml index 68fca44bcc723..1759c17a5ab6b 100644 --- a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml +++ b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -1,15 +1,12 @@ - - - - + - + diff --git a/perception/radar_object_clustering/package.xml b/perception/radar_object_clustering/package.xml index 49214b429d052..128197d8dfc47 100644 --- a/perception/radar_object_clustering/package.xml +++ b/perception/radar_object_clustering/package.xml @@ -7,6 +7,7 @@ Sathshi Tanaka Shunsuke Miura Yoshi Ri + Taekjin Lee Apache License 2.0 Sathshi Tanaka diff --git a/perception/radar_tracks_msgs_converter/CMakeLists.txt b/perception/radar_tracks_msgs_converter/CMakeLists.txt index 2a7090eebb996..766fb4b939904 100644 --- a/perception/radar_tracks_msgs_converter/CMakeLists.txt +++ b/perception/radar_tracks_msgs_converter/CMakeLists.txt @@ -24,5 +24,6 @@ endif() ## Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index fa08eb08f521a..b55755fb96f92 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -7,31 +7,15 @@ This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-p ## Design -### Input / Output +### Background -- Input - - `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic - - `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic -- Output - - `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message. This is used for radar sensor fusion detection and radar detection. - - `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message. This is used for tracking layer sensor fusion. +Autoware uses [radar_msgs/msg/RadarTracks.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) as radar objects input data. +To use radar objects data for Autoware perception module easily, `radar_tracks_msgs_converter` converts message type from `radar_msgs/msg/RadarTracks.msg` to `autoware_auto_perception_msgs/msg/DetectedObject`. +In addition, because many detection module have an assumption on base_link frame, `radar_tracks_msgs_converter` provide the functions of transform frame_id. -### Parameters - -- `update_rate_hz` (double): The update rate [hz]. - - Default parameter is 20.0 -- `new_frame_id` (string): The header frame of the output topic. - - Default parameter is "base_link" -- `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion. - - Default parameter is "true" -- `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. - - Default parameter is "false" -- `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s]. - - Default parameter is 1.0 - -## Note +### Note -This package convert the label from `radar_msgs/msg/RadarTrack.msg` to Autoware label. +`Radar_tracks_msgs_converter` converts the label from `radar_msgs/msg/RadarTrack.msg` to Autoware label. Label id is defined as below. | | RadarTrack | Autoware | @@ -45,6 +29,54 @@ Label id is defined as below. | BICYCLE | 32006 | 6 | | PEDESTRIAN | 32007 | 7 | -- [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg): additional vendor-specific classifications are permitted starting from 32000. +Additional vendor-specific classifications are permitted starting from 32000 in [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg). +Autoware objects label is defined in [ObjectClassification.idl](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl) + +## Interface + +### Input + +- `~/input/radar_objects` (`radar_msgs/msg/RadarTracks.msg`) + - Input radar topic +- `~/input/odometry` (`nav_msgs/msg/Odometry.msg`) + - Ego vehicle odometry topic + +### Output + +- `~/output/radar_detected_objects` (`autoware_auto_perception_msgs/msg/DetectedObject.idl`) + - DetectedObject topic converted to Autoware message. + - This is used for radar sensor fusion detection and radar detection. +- `~/output/radar_tracked_objects` (`autoware_auto_perception_msgs/msg/TrackedObject.idl`) + - TrackedObject topic converted to Autoware message. + - This is used for tracking layer sensor fusion. + +### Parameters + +- `update_rate_hz` (double) [hz] + - Default parameter is 20.0 + +This parameter is update rate for the `onTimer` function. +This parameter should be same as the frame rate of input topics. + +- `new_frame_id` (string) + - Default parameter is "base_link" + +This parameter is the header frame_id of the output topic. + +- `use_twist_compensation` (bool) + - Default parameter is "true" + +This parameter is the flag to use the compensation to linear of ego vehicle's twist. +If the parameter is true, then the twist of the output objects' topic is compensated by the ego vehicle linear motion. + +- `use_twist_yaw_compensation` (bool) + - Default parameter is "false" + +This parameter is the flag to use the compensation to yaw rotation of ego vehicle's twist. +If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. + +- `static_object_speed_threshold` (float) [m/s] + - Default parameter is 1.0 -- [Autoware objects label](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl) +This parameter is the threshold to determine the flag `is_stationary`. +If the velocity is lower than this parameter, the flag `is_stationary` of DetectedObject is set to `true` and dealt as a static object. diff --git a/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml new file mode 100644 index 0000000000000..5a30a6bb4031d --- /dev/null +++ b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + use_twist_compensation: true + use_twist_yaw_compensation: false + static_object_speed_threshold: 1.0 diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index cc2bb80c6f4f7..1aa4d51703b0c 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -3,19 +3,13 @@ - - - - + - - - - + diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml index 4092125cde33f..dbd900d03cbfd 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/radar_tracks_msgs_converter/package.xml @@ -6,6 +6,9 @@ radar_tracks_msgs_converter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Satoshi Tanaka diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 65d1944417cc0..f13b5d1e5f273 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -4,9 +4,6 @@ - - - diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/simple_object_merger/CMakeLists.txt index 77b10c0afb87a..10bd0efa7b8fc 100644 --- a/perception/simple_object_merger/CMakeLists.txt +++ b/perception/simple_object_merger/CMakeLists.txt @@ -25,5 +25,6 @@ endif() # Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 4609638cc008c..ec3c03424da52 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -1,60 +1,90 @@ # simple_object_merger -This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without data association algorithm. +This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) with low calculation cost. -## Algorithm +## Design ### Background -This package can merge multiple DetectedObjects without matching processing. -[Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. -In addition, for now, [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/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](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) nodes need to stand. +[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. -So this package aim to merge DetectedObjects simply. -This 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. +Therefore, `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. ### Limitation - Sensor data drops and delay -Merged objects will not be published until all topic data is received when initializing. -In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout. -When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects. -For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout. -The timeout parameter should be determined by sensor cycle time. +Merged objects will not be published until all topic data is received when initializing. In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout. When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects. For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout.The timeout parameter should be determined by sensor cycle time. - Post-processing -Because this package does not have matching processing, so it can be used only when post-processing is used. -For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. +Because this package does not have matching processing, there are overlapping objects depending on the input objects. So output objects can be used only when post-processing is used. For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. -### Use case +## Interface -Use case is as below. +### Input -- Multiple radar detection +Input topics is defined by the parameter of `input_topics` (List[string]). The type of input topics is `std::vector`. + +### Output + +- `~/output/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Merged objects combined from input topics. + +### Parameters + +- `update_rate_hz` (double) [hz] + - Default parameter: 20.0 + +This parameter is update rate for the `onTimer` function. +This parameter should be same as the frame rate of input topics. + +- `new_frame_id` (string) + - Default parameter: "base_link" + +This parameter is the header frame_id of the output topic. +If output topics use for perception module, it should be set for "base_link" -This package can be used for multiple radar detection. -Since [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) will be included later process in radar faraway detection, this package can be used. +- `timeout_threshold` (double) [s] + - Default parameter: 0.1 -## Input +This parameter is the threshold for timeout judgement. +If the time difference between the first topic of `input_topics` and an input topic is exceeded to this parameter, then the objects of topic is not merged to output objects. -| Name | Type | Description | -| ---- | ------------------------------------------------------------------ | ------------------------------------------------------ | -| | std::vector | 3D detected objects. Topic names are set by parameters | +```cpp + for (size_t i = 0; i < input_topic_size; i++) { + double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - + rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); + if (std::abs(time_diff) < node_param_.timeout_threshold) { + // merge objects + } + } +``` -## Output +- `input_topics` (List[string]) + - Default parameter: "[]" -| Name | Type | Description | -| ------------------ | ----------------------------------------------------- | -------------- | -| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Merged objects | +This parameter is the name of input topics. +For example, when this packages use for radar objects, -## Parameters +```yaml +input_topics: + [ + "/sensing/radar/front_center/detected_objects", + "/sensing/radar/front_left/detected_objects", + "/sensing/radar/rear_left/detected_objects", + "/sensing/radar/rear_center/detected_objects", + "/sensing/radar/rear_right/detected_objects", + "/sensing/radar/front_right/detected_objects", + ] +``` -| Name | Type | Description | Default value | -| :------------------ | :----------- | :----------------------------------- | :------------ | -| `update_rate_hz` | double | Update rate. [hz] | 20.0 | -| `new_frame_id` | string | The header frame_id of output topic. | "base_link" | -| `timeout_threshold` | double | Threshold for timeout judgement [s]. | 1.0 | -| `input_topics` | List[string] | Input topics name. | "[]" | +can be set in config yaml file. +For now, the time difference is calculated by the header time between the first topic of `input_topics` and the input topics, so the most important objects to detect should be set in the first of `input_topics` list. diff --git a/perception/simple_object_merger/config/simple_object_merger.param.yaml b/perception/simple_object_merger/config/simple_object_merger.param.yaml new file mode 100644 index 0000000000000..b285a3110d22d --- /dev/null +++ b/perception/simple_object_merger/config/simple_object_merger.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + new_frame_id: "base_link" + timeout_threshold: 1.0 + input_topics: [""] diff --git a/perception/simple_object_merger/launch/simple_object_merger.launch.xml b/perception/simple_object_merger/launch/simple_object_merger.launch.xml index 14fca01fa6438..43e22ea081feb 100644 --- a/perception/simple_object_merger/launch/simple_object_merger.launch.xml +++ b/perception/simple_object_merger/launch/simple_object_merger.launch.xml @@ -1,18 +1,9 @@ - - - - - - + - - - - - + diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 693fccb7e937c..683c6921eef5b 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -82,7 +82,7 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ // Node Parameter node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); - node_param_.timeout_threshold = declare_parameter("timeout_threshold", 1.0); + node_param_.timeout_threshold = declare_parameter("timeout_threshold", 0.1); declare_parameter("input_topics", std::vector()); node_param_.topic_names = get_parameter("input_topics").as_string_array(); diff --git a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp index af8065ffed379..a64c94c008526 100644 --- a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp +++ b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -67,22 +68,6 @@ struct Deleter template using unique_ptr = std::unique_ptr; -class Logger : public nvinfer1::ILogger -{ -public: - explicit Logger(bool verbose) : verbose_(verbose) {} - - void log(Severity severity, const char * msg) noexcept override - { - if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { - std::cout << msg << std::endl; - } - } - -private: - bool verbose_{false}; -}; - struct Config { int num_anchors; @@ -105,7 +90,7 @@ class Net Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, const Config & yolo_config, const std::vector & calibration_images, - const std::string & calibration_table, bool verbose = false, + const std::string & calibration_table, bool verbose = true, size_t workspace_size = (1ULL << 30)); ~Net(); @@ -138,6 +123,7 @@ class Net cuda::unique_ptr out_scores_d_ = nullptr; cuda::unique_ptr out_boxes_d_ = nullptr; cuda::unique_ptr out_classes_d_ = nullptr; + tensorrt_common::Logger logger_; void load(const std::string & path); bool prepare(); diff --git a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp index 7f1d2dbbf4577..c8793aa4c8512 100644 --- a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp +++ b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp @@ -113,10 +113,9 @@ std::vector Net::preprocess( return result; } -Net::Net(const std::string & path, bool verbose) +Net::Net(const std::string & path, bool verbose) : logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); load(path); if (!prepare()) { std::cout << "Fail to prepare engine" << std::endl; @@ -136,9 +135,9 @@ Net::Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, const Config & yolo_config, const std::vector & calibration_images, const std::string & calibration_table, bool verbose, size_t workspace_size) +: logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { std::cout << "Fail to create runtime" << std::endl; return; @@ -147,7 +146,7 @@ Net::Net( bool int8 = precision.compare("INT8") == 0; // Create builder - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger)); + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return; @@ -168,20 +167,23 @@ Net::Net( #endif // Parse ONNX FCN - std::cout << "Building " << precision << " core model..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Building " << precision << " core model..." << std::endl; const auto flag = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); auto network = unique_ptr(builder->createNetworkV2(flag)); if (!network) { - std::cout << "Fail to create network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create network" << std::endl; return; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger)); + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); if (!parser) { - std::cout << "Fail to create parser" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create parser" << std::endl; return; } + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); parser->parseFromFile( onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); std::vector scores, boxes, classes; @@ -250,28 +252,31 @@ Net::Net( calib = std::make_unique(stream, calibration_table); config->setInt8Calibrator(calib.get()); } else { - std::cout << "Fail to find enough images for INT8 calibration. Build FP mode..." << std::endl; + tensorrt_common::LOG_WARN(logger_) + << "Fail to find enough images for INT8 calibration. Build FP mode..." << std::endl; } } // Build engine - std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - std::cout << "Fail to create serialized network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create serialized network" << std::endl; + logger_.stop_throttle(log_thread); return; } engine_ = unique_ptr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!prepare()) { - std::cout << "Fail to create engine" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create engine" << std::endl; + logger_.stop_throttle(log_thread); return; } + logger_.stop_throttle(log_thread); } void Net::save(const std::string & path) const { - std::cout << "Writing to " << path << "..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Writing to " << path << "..." << std::endl; std::ofstream file(path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); } diff --git a/perception/tensorrt_yolo/package.xml b/perception/tensorrt_yolo/package.xml index 8a6756449b70f..12d07647093b5 100755 --- a/perception/tensorrt_yolo/package.xml +++ b/perception/tensorrt_yolo/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components sensor_msgs + tensorrt_common tier4_perception_msgs ament_lint_auto diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml index 702809b3cede1..c232dbde40b51 100644 --- a/perception/tracking_object_merger/config/data_association_matrix.param.yaml +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -3,7 +3,7 @@ lidar-lidar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -59,7 +59,7 @@ lidar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -115,7 +115,7 @@ radar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml index 4a108be657a27..92a7fca229818 100644 --- a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml +++ b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml @@ -23,4 +23,4 @@ # logging settings enable_logging: false - log_file_path: "/tmp/decorative_tracker_merger.log" + logging_file_path: "association_log.json" diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml index 5affbe474e8ae..52b0841f97e97 100644 --- a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -5,8 +5,6 @@ - - @@ -15,7 +13,5 @@ - - diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 81f63538f9e22..85ab105e61038 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -484,9 +484,9 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su { if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { // warning - std::cerr << "[object_tracking_merger]: motion direction is different in " - "updateWholeTrackedObject function." - << std::endl; + // std::cerr << "[object_tracking_merger]: motion direction is different in " + // "updateWholeTrackedObject function." + // << std::endl; } main_obj = sub_obj; } diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 343531f4a00f1..e96f21b8ff01b 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 6e32d3410c260..d08378f3dd129 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -1,7 +1,7 @@ - + @@ -12,7 +12,7 @@ - + diff --git a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml index 714c4d288b603..ad2851ced39c6 100644 --- a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml +++ b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp index 52d411c253fe5..00c21cdcaf29e 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp @@ -15,6 +15,8 @@ #ifndef TRT_SSD_HPP_ #define TRT_SSD_HPP_ +#include + #include <./cuda_runtime.h> #include @@ -39,22 +41,6 @@ struct Deleter template using unique_ptr = std::unique_ptr; -class Logger : public nvinfer1::ILogger -{ -public: - explicit Logger(bool verbose) : verbose_(verbose) {} - - void log(Severity severity, const char * msg) noexcept override - { - if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { - std::cout << msg << std::endl; - } - } - -private: - bool verbose_{false}; -}; - struct Shape { int channel, width, height; @@ -78,7 +64,7 @@ class Net // Create engine from serialized onnx model Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, - bool verbose = false, size_t workspace_size = (1ULL << 30)); + bool verbose = true, size_t workspace_size = (1ULL << 30)); ~Net(); @@ -127,6 +113,7 @@ class Net unique_ptr engine_ = nullptr; unique_ptr context_ = nullptr; cudaStream_t stream_ = nullptr; + tensorrt_common::Logger logger_; void load(const std::string & path); void prepare(); diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp index 22aa0f6bf3880..8aceb32d2ec58 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -52,10 +52,9 @@ void Net::prepare() cudaStreamCreate(&stream_); } -Net::Net(const std::string & path, bool verbose) +Net::Net(const std::string & path, bool verbose) : logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); load(path); prepare(); } @@ -70,9 +69,9 @@ Net::~Net() Net::Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, bool verbose, size_t workspace_size) +: logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { std::cout << "Fail to create runtime" << std::endl; return; @@ -81,7 +80,7 @@ Net::Net( bool int8 = precision.compare("INT8") == 0; // Create builder - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger)); + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return; @@ -102,19 +101,22 @@ Net::Net( #endif // Parse ONNX FCN - std::cout << "Building " << precision << " core model..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Building " << precision << " core model..." << std::endl; const auto flag = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); auto network = unique_ptr(builder->createNetworkV2(flag)); if (!network) { - std::cout << "Fail to create network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create network" << std::endl; return; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger)); + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); if (!parser) { - std::cout << "Fail to create parser" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create parser" << std::endl; return; } + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); parser->parseFromFile( onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); @@ -141,28 +143,31 @@ Net::Net( config->addOptimizationProfile(profile); // Build engine - std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - std::cout << "Fail to create serialized network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create serialized network" << std::endl; + logger_.stop_throttle(log_thread); return; } engine_ = unique_ptr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - std::cout << "Fail to create engine" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create engine" << std::endl; + logger_.stop_throttle(log_thread); return; } context_ = unique_ptr(engine_->createExecutionContext()); if (!context_) { - std::cout << "Fail to create context" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create context" << std::endl; + logger_.stop_throttle(log_thread); return; } + logger_.stop_throttle(log_thread); } void Net::save(const std::string & path) { - std::cout << "Writing to " << path << "..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Writing to " << path << "..." << std::endl; std::ofstream file(path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); } diff --git a/perception/traffic_light_ssd_fine_detector/package.xml b/perception/traffic_light_ssd_fine_detector/package.xml index 994f84bacdae4..cd44857e8fe86 100644 --- a/perception/traffic_light_ssd_fine_detector/package.xml +++ b/perception/traffic_light_ssd_fine_detector/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components sensor_msgs + tensorrt_common tier4_debug_msgs tier4_perception_msgs diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 6107314bc2824..32c92b4f4b88b 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -173,11 +173,9 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto shorten_lanes = utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, true)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, false)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false)); // get related objects from dynamic_objects, and then separates them as target objects and non // target objects diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index b300de2236fcf..d9ae9ed623cb3 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -19,6 +19,7 @@ use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true + use_freespace_areas: true # for debug publish_debug_marker: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 9b5ae3cb4db9e..5e162a17a5064 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -115,6 +115,9 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // use freespace area for avoidance + bool use_freespace_areas{false}; + // consider avoidance return dead line bool enable_dead_line_for_goal{false}; bool enable_dead_line_for_traffic_light{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index f5b797978b744..0f72c1aefc33b 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -261,10 +261,11 @@ class AvoidanceHelper bool isComfortable(const AvoidLineArray & shift_lines) const { + const auto JERK_BUFFER = 0.1; // [m/sss] return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { return PathShifter::calcJerkFromLatLonDistance( line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < - getLateralMaxJerkLimit(); + getLateralMaxJerkLimit() + JERK_BUFFER; }); } diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 9f2fdf7ab96d9..03902e4328eb2 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -57,6 +57,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + p.use_freespace_areas = getOrDeclareParameter(*node, ns + "use_freespace_areas"); } // target object diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 0bb480bfa26b1..a74c3a69d7bce 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -78,8 +78,6 @@ class AvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } - /** * @brief update RTC status for candidate shift line. * @param candidate path. diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index f088b9228d964..fcca9b1a18f49 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -47,7 +47,7 @@ int32_t uuidToInt32(const unique_identifier_msgs::msg::UUID & uuid) int32_t ret = 0; for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); + ret <<= sizeof(int8_t) * 8; ret |= uuid.uuid.at(i); } @@ -557,6 +557,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("TooNearToGoal")); addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("UnstableObject")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index bc1d8c6d75424..5172bcbeb626c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -235,11 +235,13 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, true)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, false)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, false)); // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -301,8 +303,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; + ObjectData other_object = createObjectData(data, object); other_object.reason = "OutOfTargetArea"; data.other_objects.push_back(other_object); } @@ -938,6 +939,8 @@ BehaviorModuleOutput AvoidanceModule::plan() // expand intersection areas current_drivable_area_info.enable_expanding_intersection_areas = parameters_->use_intersection_areas; + // expand freespace areas + current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 6196b3e209d11..96e22152a2f93 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -185,29 +185,36 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // prepare distance is not enough. unavoidable. - if (remaining_distance < 1e-3) { + if (avoidance_distance < 1e-3) { object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; return std::nullopt; } // calculate lateral jerk. const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( - avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed()); + avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. if (required_jerk < helper_->getLateralMaxJerkLimit()) { return std::make_pair(desire_shift_length, avoidance_distance); } + constexpr double LON_DIST_BUFFER = 1e-3; + // avoidance distance is not enough. unavoidable. if (!isBestEffort(parameters_->policy_deceleration)) { - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return std::nullopt; + if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } else { + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return std::nullopt; + } } // output avoidance path under lateral jerk constraints. const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( - remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); + avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { object.reason = "LessThanExecutionThreshold"; @@ -218,8 +225,17 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift : -1.0 * feasible_relative_shift_length + current_ego_shift; + if ( + avoidance_distance < + helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } + + const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; + const auto infeasible = - std::abs(feasible_shift_length - object.overhang_dist) < + std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER < 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); @@ -227,7 +243,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return std::nullopt; } - return std::make_pair(feasible_shift_length, avoidance_distance); + return std::make_pair(feasible_shift_length - LAT_DIST_BUFFER, avoidance_distance); }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 3a9b5db283304..2bc909c115c87 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -21,6 +21,7 @@ #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include #include #include @@ -368,13 +369,15 @@ bool isSafetyCheckTargetObjectType( return parameters->object_parameters.at(object_type).is_safety_check_target; } -bool isVehicleTypeObject(const ObjectData & object) +bool isUnknownTypeObject(const ObjectData & object) { const auto object_type = utils::getHighestProbLabel(object.object.classification); + return object_type == ObjectClassification::UNKNOWN; +} - if (object_type == ObjectClassification::UNKNOWN) { - return false; - } +bool isVehicleTypeObject(const ObjectData & object) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); if (object_type == ObjectClassification::PEDESTRIAN) { return false; @@ -387,6 +390,14 @@ bool isVehicleTypeObject(const ObjectData & object) return true; } +bool isMovingObject( + const ObjectData & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + return object.move_time > object_parameter.moving_time_threshold; +} + bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs) @@ -474,11 +485,11 @@ bool isObjectOnRoadShoulder( ObjectData & object, const std::shared_ptr & route_handler, const std::shared_ptr & parameters) { - using boost::geometry::return_centroid; using boost::geometry::within; using lanelet::geometry::distance2d; using lanelet::geometry::toArcCoordinates; using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; // assume that there are no parked vehicles in intersection. std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); @@ -496,13 +507,9 @@ bool isObjectOnRoadShoulder( // +: object position // o: nearest point on centerline - lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + const auto centerline_pos = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -524,8 +531,9 @@ bool isObjectOnRoadShoulder( } } - const auto center_to_left_boundary = - distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + const auto center_to_left_boundary = distance2d( + to2D(most_left_lanelet.leftBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); return std::make_pair( center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); @@ -536,7 +544,8 @@ bool isObjectOnRoadShoulder( } const auto arc_coordinates = toArcCoordinates( - to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + to2D(object.overhang_lanelet.centerline().basicLineString()), + to2D(toLaneletPoint(object_pose.position)).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -562,8 +571,9 @@ bool isObjectOnRoadShoulder( } } - const auto center_to_right_boundary = - distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); + const auto center_to_right_boundary = distance2d( + to2D(most_right_lanelet.rightBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); return std::make_pair( center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); @@ -574,7 +584,8 @@ bool isObjectOnRoadShoulder( } const auto arc_coordinates = toArcCoordinates( - to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + to2D(object.overhang_lanelet.centerline().basicLineString()), + to2D(toLaneletPoint(object_pose.position)).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = @@ -663,9 +674,7 @@ bool isSatisfiedWithCommonCondition( } // Step2. filtered stopped objects. - const auto object_type = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); - if (object.move_time > object_parameter.moving_time_threshold) { + if (filtering_utils::isMovingObject(object, parameters)) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return false; } @@ -723,6 +732,15 @@ bool isSatisfiedWithNonVehicleCondition( return false; } + // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + return true; } @@ -743,6 +761,8 @@ bool isSatisfiedWithVehicleCondition( const std::shared_ptr & parameters) { using boost::geometry::within; + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; object.behavior = getObjectBehavior(object, parameters); object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); @@ -753,9 +773,10 @@ bool isSatisfiedWithVehicleCondition( } // check vehicle shift ratio - lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); - const auto on_ego_driving_lane = - within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + const auto on_ego_driving_lane = within( + to2D(toLaneletPoint(object_pos)).basicPoint(), + object.overhang_lanelet.polygon2d().basicPolygon()); if (on_ego_driving_lane) { if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) { return true; @@ -1626,6 +1647,16 @@ void filterTargetObjects( o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] + if (filtering_utils::isUnknownTypeObject(o)) { + if (o.stop_time < STOP_TIME_THRESHOLD) { + o.reason = "UnstableObject"; + data.other_objects.push_back(o); + continue; + } + } + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue; @@ -2278,11 +2309,15 @@ TurnSignalInfo calcTurnSignalInfo( return {}; } - const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true); - const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true); + const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); + const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); + const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); + const auto has_right_lane = + right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); - if (!existShiftSideLane( - start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) { + if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) { return {}; } diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index a5380b628387d..696df7f7971ea 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -345,7 +345,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + ModuleStatus setInitState() const override { return ModuleStatus::IDLE; } bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index b43102ecbd16d..73cabd00592a8 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -397,6 +397,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + output.modified_goal = getPreviousModuleOutput().modified_goal; return output; } diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index 6bd2f8b9c79e4..dbdb540b2ead4 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -4,8 +4,8 @@ Plan path around the goal. -- Park at the designated goal. -- Modify the goal to avoid obstacles or to pull over at the side of tha lane. +- Arrive at the designated goal. +- Modify the goal to avoid obstacles or to pull over at the side of the lane. ## Design @@ -29,13 +29,13 @@ package goal_planner{ class FreeSpacePullOver {} } - class GoalSeacher {} + class GoalSearcher {} struct GoalCandidates {} struct PullOverPath{} abstract class PullOverPlannerBase {} - abstract class GoalSeacherBase {} + abstract class GoalsearcherBase {} } @@ -62,7 +62,7 @@ package freespace_planning_algorithms ShiftPullOver --|> PullOverPlannerBase GeometricPullOver --|> PullOverPlannerBase FreeSpacePullOver --|> PullOverPlannerBase -GoalSeacher --|> GoalSeacherBase +GoalSearcher --|> GoalSearcherBase DefaultFixedPlanner --|> FixedGoalPlannerBase PathShifter --o ShiftPullOver @@ -71,23 +71,27 @@ AstarSearch --o FreeSpacePullOver RRTStar --o FreeSpacePullOver PullOverPlannerBase --o GoalPlannerModule -GoalSeacherBase --o GoalPlannerModule +GoalSearcherBase --o GoalPlannerModule FixedGoalPlannerBase --o GoalPlannerModule PullOverPath --o PullOverPlannerBase -GoalCandidates --o GoalSeacherBase +GoalCandidates --o GoalSearcherBase @enduml ``` ## start condition -Either one is activated when all conditions are met. - ### fixed_goal_planner -- Route is set with `allow_goal_modification=false` by default. -- ego-vehicle is in the same lane as the goal. +This is a very simple function that plans a smooth path to a specified goal. This function does not require approval and always runs with the other modules. +_NOTE: this planner does not perform the several features described below, such as "goal search", "collision check", "safety check", etc._ + +Executed when both conditions are met. + +- Route is set with `allow_goal_modification=false`. This is the default. +- The goal is set in the normal lane. In other words, it is NOT `road_shoulder`. +- Ego-vehicle exists in the same lane sequence as the goal. If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. @@ -129,61 +133,78 @@ If the target path contains a goal, modify the points of the path so that the pa | th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | -## **collision check** +## **Goal Search** -### **occupancy grid based collision check** +To realize pull over even when an obstacle exists near the original goal, a collision free area is searched within a certain range around the original goal. The goal found will be published as `/planning/scenario_planning/modified_goal`. -Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. +[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) -#### Parameters for occupancy grid based collision check +1. The original goal is set, and the refined goal pose is obtained by moving in the direction normal to the lane center line and keeping `margin_from_boundary` from the edge of the lane. + ![refined_goal](./images/goal_planner-refined_goal.drawio.svg) -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | -| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +2. Using `refined_goal` as the base goal, search for candidate goals in the range of `-forward_goal_search_length` to `backward_goal_search_length` in the longitudinal direction and `longitudinal_margin` to `longitudinal_margin+max_lateral_offset` in th lateral direction based on refined_goal. + ![goal_candidates](./images/goal_planner-goal_candidates.drawio.svg) -### **object recognition based collision check** +3. Each candidate goal is prioritized and a path is generated for each planner for each goal. The priority of a candidate goal is determined by its distance from the base goal. The ego vehicle tries to park for the highest possible goal. The distance is determined by the selected policy. In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance. This means the distance is calculated by `longitudinal_distance + lateral_cost*lateral_distance` + ![goal_distance](./images/goal_planner-goal_distance.drawio.svg) + The following figure is an example of minimum_weighted_distance.​ The white number indicates the goal candidate priority, and the smaller the number, the higher the priority. the 0 goal indicates the base goal. + ![goal_priority_rviz_with_goal](./images/goal_priority_with_goal.png) + ![goal_priority_rviz](./images/goal_priority_rviz.png) -#### Parameters for object recognition based collision check +4. If the footprint in each goal candidate is within `object_recognition_collision_check_margin` of that of the object, it is determined to be unsafe. These goals are not selected. If `use_occupancy_grid_for_goal_search` is enabled, collision detection on the grid is also performed with `occupancy_grid_collision_check_margin`. -| Name | Unit | Type | Description | Default value | -| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | -| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | -| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | +Red goals candidates in the image indicate unsafe ones. -## **Goal Search** +![is_safe](./images/goal_planner-is_safe.drawio.svg) -If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is -searched for in certain range of the shoulder lane. +It is possible to keep `longitudinal_margin` in the longitudinal direction apart from the collision margin for obstacles from the goal candidate. This is intended to provide natural spacing for parking and efficient departure. -[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) +![longitudinal_margin](./images/goal_planner-longitudinal_margin.drawio.svg) + +Also, if `prioritize_goals_before_objects` is enabled, To arrive at each goal, the number of objects that need to be avoided in the target range is counted, and those with the lowest number are given priority. + +The images represent a count of objects to be avoided at each range, with priority given to those with the lowest number, regardless of the aforementioned distances. + +![object_to_avoid](./images/goal_planner-object_to_avoid.drawio.svg) + +The gray numbers represent objects to avoid, and you can see that the goal in front has a higher priority in this case. + +![goal_priority_object_to_avoid_rviz.png](./images/goal_priority_object_to_avoid_rviz.png) ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | -| goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | -| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. +The path is generated with a certain margin (default: `0.75 m`) from the boundary of shoulder lane. + +The process is time consuming because multiple planners are used to generate path for each candidate goal. Therefore, in this module, the path generation is performed in a thread different from the main thread. +Path generation is performed at the timing when the shape of the output path of the previous module changes. If a new module launches, it is expected to go to the previous stage before the goal planner, in which case the goal planner re-generates the path. The goal planner is expected to run at the end of multiple modules, which is achieved by `keep_last` in the planner manager. + +Threads in the goal planner are shown below. + +![threads.png](./images/goal_planner-threads.drawio.svg) + +The main thread will be the one called from the planner manager flow. + +- The goal candidate generation and path candidate generation are done in a separate thread(lane path generation thread). +- The path candidates generated there are referred to by the main thread, and the one judged to be valid for the current planner data (e.g. ego and object information) is selected from among them. valid means no sudden deceleration, no collision with obstacles, etc. The selected path will be the output of this module. +- If there is no path selected, or if the selected path is collision and ego is stuck, a separate thread(freespace path generation thread) will generate a path using freespace planning algorithm. If a valid free space path is found, it will be the output of the module. If the object moves and the pull over path generated along the lane is collision-free, the path is used as output again. See also the section on freespace parking for more information on the flow of generating freespace paths. | Name | Unit | Type | Description | Default value | | :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | @@ -276,12 +297,6 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop -#### Unimplemented parts / limitations for freespace parking - -- When a short path is generated, the ego does can not drive with it. -- Complex cases take longer to generate or fail. -- The drivable area is not guaranteed to fit in the parking_lot. - #### Parameters freespace parking | Name | Unit | Type | Description | Default value | @@ -289,3 +304,125 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop | enable_freespace_parking | [-] | bool | This flag enables freespace parking, which runs when the vehicle is stuck due to e.g. obstacles in the parking area. | true | See [freespace_planner](../freespace_planner/README.md) for other parameters. + +## **collision check for path generation** + +To select a safe one from the path candidates, a collision check with obstacles is performed. + +### **occupancy grid based collision check** + +Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. + +#### Parameters for occupancy grid based collision check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | + +### **object recognition based collision check** + +A collision decision is made for each of the path candidates, and a collision-free path is selected. +There are three main margins at this point. + +- `object_recognition_collision_check_margin` is margin in all directions of ego. +- In the forward direction, a margin is added by the braking distance calculated from the current speed and maximum deceleration. The maximum distance is The maximum value of the distance is suppressed by the `object_recognition_collision_check_max_extra_stopping_margin` +- In curves, the lateral margin is larger than in straight lines.This is because curves are more prone to control errors or to fear when close to objects (The maximum value is limited by `object_recognition_collision_check_max_extra_stopping_margin`, although it has no basis.) + +![collision_check_margin](./images/goal_planner-collision_check_margin.drawio.svg) + +Then there is the concept of soft and hard margins. Although not currently parameterized, if a collision-free path can be generated by a margin several times larger than `object_recognition_collision_check_margin`, then the priority is higher. + +#### Parameters for object recognition based collision check + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | + +## **safety check** + +Perform safety checks on moving objects. If the object is determined to be dangerous, no path decision is made and no approval is given, + +- path decision is not made and approval is not granted. +- After approval, the ego vehicle stops under deceleration and jerk constraints. + +This module has two methods of safety check, `RSS` and `integral_predicted_polygon`. + +`RSS` method is a method commonly used by other behavior path planner modules, see [RSS based safety check utils explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md). + +`integral_predicted_polygon` is a more safety-oriented method. This method is implemented because speeds during pull over are lower than during driving, and fewer objects travel along the edge of the lane. (It is sometimes too reactive and may be less available.) +This method integrates the footprints of egos and objects at a given time and checks for collisions between them. + +![safety_check](./images/goal_planner-safety_check.drawio.svg) + +In addition, the safety check has a time hysteresis, and if the path is judged "safe" for a certain period of time(`keep_unsafe_time`), it is finally treated as "safe". + +```txt + 　　 ==== is_safe + 　　 ---- current_is_safe + 　　 is_safe + 　　 ^ + 　　 | + 　　 | time + 　　 1 +--+ +---+ +---========= +--+ + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 | | | | | | | | + 　　 0 =========================-------==========--> t +``` + +### Parameters for safety check + +| Name | Unit | Type | Description | Default value | +| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | +| enable_safety_check | [-] | bool | flag whether to use safety check | true | +| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | +| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | + +#### Parameters for RSS safety check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------- | :--- | :----- | :-------------------------------------- | :------------ | +| rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | + +#### Parameters for integral_predicted_polygon safety check + +| Name | Unit | Type | Description | Default value | +| :-------------- | :--- | :----- | :------------------------------------- | :------------ | +| forward_margin | [m] | double | forward margin for ego footprint | 1.0 | +| backward_margin | [m] | double | backward margin for ego footprint | 1.0 | +| lat_margin | [m] | double | lateral margin for ego footprint | 1.0 | +| time_horizon | [s] | double | Time width to integrate each footprint | 10.0 | + +## **path deciding** + +When `decide_path_distance` closer to the start of the pull over, if it is collision-free at that time and safe for the predicted path of the objects, it transitions to DECIDING. If it is safe for a certain period of time, it moves to DECIDED. + +![path_deciding](./images/goal_planner-deciding_path.drawio.svg) + +## Unimplemented parts / limitations + +- Only shift pull over can be executed concurrently with other modules +- Parking in tight spots and securing margins are traded off. A mode is needed to reduce the margin by using a slower speed depending on the situation, but there is no mechanism for dynamic switching of speeds. +- Parking space available depends on visibility of objects, and sometimes parking decisions cannot be made properly. +- Margin to unrecognized objects(Not even unknown objects) depends on the occupancy grid. May get too close to unrecognized ground objects because the objects that are allowed to approach (e.g., grass, leaves) are indistinguishable. + +Unimplemented parts / limitations for freespace parking + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg new file mode 100644 index 0000000000000..04b53eadaec04 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ collision_check_margin +
+
+
+ + + + + + + + +
+
+
+ extra_stopping_margin(v, max_decel) +
+
+
+ + + + + + + + +
+
+
+ extra_lateral_margin(v, κ) +
+
+
+ + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg new file mode 100644 index 0000000000000..38f3c4917c41c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg @@ -0,0 +1,121 @@ + + + + + + + + + + + + + +
+
+
+ NOT DECIDED +
+
+
+ + NOT DECIDED + + + + + + + + + + +
+
+
+ DECIDING +
+
+
+ + DECIDING + + + + + + +
+
+
+ DECIDED +
+
+
+ + DECIDED + + + + + + +
+
+
not safe
+
+
+ + not safe + + + + + + +
+
+
+ safe for a certain time +
+
+
+ + safe for a certain ti... + + + + + + +
+
+
safe & close to pull over start point
+
+
+ + safe & close to pull over start poi... + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg new file mode 100644 index 0000000000000..67b2f89a99bb0 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg @@ -0,0 +1,585 @@ + + + + + + + + + + + + + + +
+
+
+ margin_from_boundary +
+
+
+ + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + +
+
+
+ + max_lateral_offset + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ forward_goal_search_length +
+
+
+ + + + + + + + + + + +
+
+
+ backward_goal_search_length +
+
+
+ + + + + + + + +
+
+
+ lateral_search_interval +
+
+
+ + + + + + + + +
+
+
+ + goal_search_interval + +
+
+
+ + + + + + + + + + + + + + +
+
+
+
refined goal
+
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg new file mode 100644 index 0000000000000..8bca6f4c7e18c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg @@ -0,0 +1,466 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + +
+
+
+
refined goal
+
+
+
+ + + + + + + + + + + + + + +
+
+
+
longidudinal distance
+
+
+
+ + + + + + + + +
+
+
+
lateral distance
+
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg new file mode 100644 index 0000000000000..d7dab102a7890 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg @@ -0,0 +1,400 @@ + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg new file mode 100644 index 0000000000000..66f021bdf887d --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ longitudinal_margin +
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg new file mode 100644 index 0000000000000..b990f3c75dcdf --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg @@ -0,0 +1,472 @@ + + + + + + + + + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + +
+
+
+ 1 +
+
+
+ + + + + + + + +
+
+
+ 2 +
+
+
+ + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg new file mode 100644 index 0000000000000..5c3f7ddcb2a03 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg @@ -0,0 +1,427 @@ + + + + + + + + + + + + + + +
+
+
+ margin_from_boundary +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
original goal
+
+
+
+ + + + + + + + +
+
+
+
refined goal
+
+
+
+ + + + + Red Car - Top View image/svg+xml Openclipart Red Car - Top View + 2010-05-19T15:02:12 + + I was thinking of Trophy ( http://trophy.sourceforge.net/index.php?body=screenshots ) when remixing this one :) + http://openclipart.org/detail/61201/red-racing-car-top-view-by-qubodup qubodup + car + clip art clipart game + game sprite + racing racing car red + red car + simple simple car sprite + transport + transportation travel video game + video game art + video game sprite + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg new file mode 100644 index 0000000000000..4fe6313a8da45 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg b/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg new file mode 100644 index 0000000000000..8b094e11bd2c7 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg @@ -0,0 +1,668 @@ + + + + + + + + + + + + + + + +
+
+
+ sort paths by priority +
+
+
+ + + + + + + + + + +
+
+
+ select highest priority collision free path +
+
+
+ + + + + + + + + + +
+
+
+ main thread +
+
+
+ + + + + + + + +
+
+
+ collsion-free path +
+
+
+ + + + + + + + +
+
+
+ collision-free paths +
+
+
+ + + + + + + + +
+
+
+ + collision-free +
+ path is found + +
+
+
+ + + + + + + + +
+
+
+ get +
+
+
+ + + + + + + + + + + + +
+
+
Yes
+
+
+ + + + + + + + + + +
+
+
+ + generate goal candidates + +
+
+
+ + + + + + + + + + +
+
+
goal candidates
+
+
+ + + + + + + + + + +
+
+
run()
+
+
+ + + + + + + + +
+
+
get
+
+
+ + + + + + + + + + +
+
+
+ lane path generation thread +
+
+
+ + + + + + + + +
+
+
+ + generate path candidates + +
+
+
+ + + + + + + + + +
+
+
+ Trigger: previous module output path  is changed. +
+
+
+ + + + + + + + +
+
+
+ onTimer() +
+
+
+ + + + + + + + +
+
+
previous module path
+
+
+ + + + + + + + +
+
+
+ pull over path candidates +
+
+
+ + + + + + + + + + +
+
+
get
+
+
+ + + + + + + + + + + + +
+
+
+ freespace path generation thead +
+
+
+ + + + + + + + +
+
+
+ + generate freespace paths + +
+
+
+ + + + + + + + + +
+
+
+ Trigger: ego vehicle is stuck +
+
+
+ + + + + + + + +
+
+
+ onFreespaceParkingTimer() +
+
+
+ + + + + + + + +
+
+
freespace path
+
+
+ + + + + + + + + + +
+
+
+ if collision-fee path is found... +
+
+
+ + + + + + + + +
+
+
get
+
+
+ + + + + + + + + + +
+
+
No
+
+
+ + + + + + + + + + diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png b/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png new file mode 100644 index 0000000000000..5067e0efc90a8 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png differ diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png b/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png new file mode 100644 index 0000000000000..dbcd3e1055497 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png differ diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png b/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png new file mode 100644 index 0000000000000..4094734cb8996 Binary files /dev/null and b/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png differ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 51638e5485fe2..20884b009667e 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -290,6 +291,35 @@ class GoalPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); + ~GoalPlannerModule() + { + if (lane_parking_timer_) { + lane_parking_timer_->cancel(); + } + if (freespace_parking_timer_) { + freespace_parking_timer_->cancel(); + } + + while (is_lane_parking_cb_running_.load() || is_freespace_parking_cb_running_.load()) { + const std::string running_callbacks = std::invoke([&]() { + if (is_lane_parking_cb_running_ && is_freespace_parking_cb_running_) { + return "lane parking and freespace parking"; + } + if (is_lane_parking_cb_running_) { + return "lane parking"; + } + return "freespace parking"; + }); + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 1000, "Waiting for %s callback to finish...", + running_callbacks.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished"); + } + void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); @@ -330,6 +360,18 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: + // Flag class for managing whether a certain callback is running in multi-threading + class ScopedFlag + { + public: + explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } + + ~ScopedFlag() { flag_.store(false); } + + private: + std::atomic & flag_; + }; + /* * state transitions and plan function used in each state * @@ -356,7 +398,6 @@ class GoalPlannerModule : public SceneModuleInterface // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; @@ -404,10 +445,12 @@ class GoalPlannerModule : public SceneModuleInterface // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; + std::atomic is_lane_parking_cb_running_; // generate freespace parking paths in a separate thread rclcpp::TimerBase::SharedPtr freespace_parking_timer_; rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; + std::atomic is_freespace_parking_cb_running_; // debug mutable GoalPlannerDebugData debug_data_; @@ -419,7 +462,7 @@ class GoalPlannerModule : public SceneModuleInterface bool checkOccupancyGridCollision(const PathWithLaneId & path) const; bool checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, - const bool update_debug_data = false) const; + const bool extract_static_objects, const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; @@ -493,6 +536,7 @@ class GoalPlannerModule : public SceneModuleInterface std::optional last_previous_module_output_{}; bool hasPreviousModulePathShapeChanged() const; bool hasDeviatedFromLastPreviousModulePath() const; + bool hasDeviatedFromCurrentPreviousModulePath() const; // timer for generating pull over path candidates in a separate thread void onTimer(); diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml index 213c0093b08d9..a3023389cdd32 100644 --- a/planning/behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index faa9e28e28b3b..b59a8f01fa4ab 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -62,6 +62,8 @@ GoalPlannerModule::GoalPlannerModule( parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_}, + is_lane_parking_cb_running_{false}, + is_freespace_parking_cb_running_{false}, debug_stop_pose_with_info_{&stop_pose_} { LaneDepartureChecker lane_departure_checker{}; @@ -161,9 +163,18 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const planner_data_->self_odometry->pose.pose.position)) > 0.3; } +bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const +{ + return std::abs(motion_utils::calcLateralOffset( + getPreviousModuleOutput().path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { + const ScopedFlag flag(is_lane_parking_cb_running_); + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -179,15 +190,19 @@ void GoalPlannerModule::onTimer() // check if new pull over path candidates are needed to be generated const bool need_update = std::invoke([&]() { + if (hasDeviatedFromCurrentPreviousModulePath()) { + RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); + return false; + } if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return true; } if (hasPreviousModulePathShapeChanged()) { - RCLCPP_ERROR(getLogger(), "has previous module path shape changed"); + RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { - RCLCPP_ERROR(getLogger(), "has deviated from last previous module path"); + RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } return false; @@ -276,6 +291,8 @@ void GoalPlannerModule::onTimer() void GoalPlannerModule::onFreespaceParkingTimer() { + const ScopedFlag flag(is_freespace_parking_cb_running_); + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -624,7 +641,9 @@ bool GoalPlannerModule::canReturnToLaneParking() if ( parameters_->use_object_recognition && - checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) { + checkObjectsCollision( + path, parameters_->object_recognition_collision_check_margin, + /*extract_static_objects=*/false)) { return false; } @@ -711,7 +730,9 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); for (const auto & scale_factor : scale_factors) { - if (!checkObjectsCollision(resampled_path, margin * scale_factor)) { + if (!checkObjectsCollision( + resampled_path, margin * scale_factor, + /*extract_static_objects=*/true)) { return margin * scale_factor; } } @@ -771,8 +792,10 @@ std::optional> GoalPlannerModule::selectP const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); if ( - parameters_->use_object_recognition && - checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) { + parameters_->use_object_recognition && checkObjectsCollision( + resampled_path, collision_check_margin, + /*extract_static_objects=*/true, + /*update_debug_data=*/true)) { continue; } @@ -914,6 +937,7 @@ bool GoalPlannerModule::hasNotDecidedPath() const DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const { const auto & prev_status = prev_data_.deciding_path_status; + const bool enable_safety_check = parameters_->safety_check_params.enable_safety_check; // Once this function returns true, it will continue to return true thereafter if (prev_status.first == DecidingPathStatus::DECIDED) { @@ -927,8 +951,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if ( - parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) { + if (enable_safety_check && !isSafePath().first && !isActivated()) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -957,13 +980,20 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5); const double margin = parameters_->object_recognition_collision_check_margin * hysteresis_factor; - if (checkObjectsCollision(parking_path, margin)) { + if (checkObjectsCollision(parking_path, margin, /*extract_static_objects=*/false)) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } + if (enable_safety_check && !isSafePath().first) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + // if enough time has passed since deciding status starts, transition to DECIDED constexpr double check_collision_duration = 1.0; const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); @@ -1406,7 +1436,7 @@ bool GoalPlannerModule::isStuck() parameters_->use_object_recognition && checkObjectsCollision( thread_safe_data_.get_pull_over_path()->getCurrentPath(), - parameters_->object_recognition_collision_check_margin)) { + /*extract_static_objects=*/false, parameters_->object_recognition_collision_check_margin)) { return true; } @@ -1519,41 +1549,31 @@ bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) bool GoalPlannerModule::checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, - const bool update_debug_data) const -{ - const auto pull_over_lane_stop_objects = - goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, parameters_->detection_bound_offset, - *(planner_data_->dynamic_object), parameters_->th_moving_object_velocity); + 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); + }); std::vector obj_polygons; - for (const auto & object : pull_over_lane_stop_objects.objects) { + for (const auto & object : target_objects.objects) { obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); } /* Expand ego collision check polygon - * - `collision_check_margin` in all directions - * - `extra_stopping_margin` in the moving direction - * - `extra_lateral_margin` in external direction of path curve - * - * - * ^ moving direction - * x - * x - * x - * +----------------------+------x--+ - * | | x | - * | +---------------+ | xx | - * | | | | xx | - * | | ego footprint |xxxxx | - * | | | | | - * | +---------------+ | extra_stopping_margin - * | margin | | - * +----------------------+ | - * | extra_lateral_margin | - * +--------------------------------+ - * + * - `collision_check_margin` is added in all directions. + * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. + * - `extra_lateral_margin` adds the lateral margin on curves. */ std::vector ego_polygons_expanded{}; const auto curvatures = motion_utils::calcCurvature(path.points); @@ -1564,19 +1584,19 @@ bool GoalPlannerModule::checkObjectsCollision( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_max_extra_stopping_margin); - double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps * - std::abs(p.point.longitudinal_velocity_mps); - extra_lateral_margin = - std::clamp(extra_lateral_margin, -extra_stopping_margin, 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. + const double extra_lateral_margin = std::min( + extra_stopping_margin, + std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto lateral_offset_pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - lateral_offset_pose, + p.point.pose, planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, planner_data_->parameters.base_link2rear + collision_check_margin, planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + - std::abs(extra_lateral_margin)); + extra_lateral_margin * 2.0); ego_polygons_expanded.push_back(ego_polygon); } @@ -1849,6 +1869,9 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( std::pair GoalPlannerModule::isSafePath() const { + if (!thread_safe_data_.get_pull_over_path()) { + return {false, false}; + } const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto & current_pose = planner_data_->self_odometry->pose.pose; const double current_velocity = std::hypot( diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index bffa5201f2882..9ff8939862c65 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -361,6 +361,13 @@ The following parameters are configurable in `lane_change.param.yaml`. The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. +#### common + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.lane_expansion.left_offset` | [m] | double | Expand the left boundary of the detection area, allowing objects previously outside on the left to be detected and registered as targets. | 0.0 | +| `safety_check.lane_expansion.right_offset` | [m] | double | Expand the right boundary of the detection area, allowing objects previously outside on the right to be detected and registered as targets. | 0.0 | + #### execution | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4e741a2b3db2c..757ccd3a3116d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -126,9 +126,9 @@ class LaneChangeInterface : public SceneModuleInterface bool canTransitFailureState() override; - bool canTransitIdleToRunningState() override; + ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; - void setObjectDebugVisualization() const; + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 9c3c371627a78..e4af48a71c8f3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -53,9 +53,11 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; - void extendOutputDrivableArea(BehaviorModuleOutput & output) override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; @@ -65,7 +67,7 @@ class NormalLaneChange : public LaneChangeBase void resetParameters() override; - TurnSignalInfo updateOutputTurnSignal() override; + TurnSignalInfo updateOutputTurnSignal() const override; bool calcAbortPath() override; @@ -89,7 +91,7 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; - bool isLaneChangeRequired() const override; + bool isLaneChangeRequired() override; bool isStoppedAtRedTrafficLight() const override; @@ -141,7 +143,11 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() override; + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + TurnSignalInfo calcTurnSignalInfo() const override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 451899fbf27e6..35d018ad3d58c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ #include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -36,7 +37,6 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -66,7 +66,7 @@ class LaneChangeBase virtual BehaviorModuleOutput generateOutput() = 0; - virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) const = 0; virtual PathWithLaneId getReferencePath() const = 0; @@ -74,13 +74,13 @@ class LaneChangeBase virtual void resetParameters() = 0; - virtual TurnSignalInfo updateOutputTurnSignal() = 0; + virtual TurnSignalInfo updateOutputTurnSignal() const = 0; virtual bool hasFinishedLaneChange() const = 0; virtual bool hasFinishedAbort() const = 0; - virtual bool isLaneChangeRequired() const = 0; + virtual bool isLaneChangeRequired() = 0; virtual bool isAbortState() const = 0; @@ -88,6 +88,8 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; + virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; + virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; @@ -137,19 +139,7 @@ class LaneChangeBase const LaneChangeStatus & getLaneChangeStatus() const { return status_; } - const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; } - - const CollisionCheckDebugMap & getDebugData() const { return object_debug_; } - - const CollisionCheckDebugMap & getAfterApprovalDebugData() const - { - return object_debug_after_approval_; - } - - const LaneChangeTargetObjects & getDebugFilteredObjects() const - { - return debug_filtered_objects_; - } + const data::lane_change::Debug & getDebugData() const { return lane_change_debug_; } const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } @@ -225,7 +215,7 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() = 0; + virtual TurnSignalInfo calcTurnSignalInfo() const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; @@ -257,12 +247,8 @@ class LaneChangeBase Direction direction_{Direction::NONE}; LaneChangeModuleType type_{LaneChangeModuleType::NORMAL}; - mutable LaneChangePaths debug_valid_path_{}; - mutable CollisionCheckDebugMap object_debug_{}; - mutable CollisionCheckDebugMap object_debug_after_approval_{}; - mutable LaneChangeTargetObjects debug_filtered_objects_{}; - mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + mutable data::lane_change::Debug lane_change_debug_; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp new file mode 100644 index 0000000000000..8ac1ba3909a50 --- /dev/null +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -0,0 +1,49 @@ +// 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 BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ + +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" + +#include + +#include + +namespace behavior_path_planner::data::lane_change +{ +using utils::path_safety_checker::CollisionCheckDebugMap; +struct Debug +{ + std::string module_type; + LaneChangePaths valid_paths; + CollisionCheckDebugMap collision_check_objects; + CollisionCheckDebugMap collision_check_objects_after_approval; + LaneChangeTargetObjects filtered_objects; + double collision_check_object_debug_lifetime{0.0}; + + void reset() + { + valid_paths.clear(); + collision_check_objects.clear(); + collision_check_objects_after_approval.clear(); + filtered_objects.current_lane.clear(); + filtered_objects.target_lane.clear(); + filtered_objects.other_lane.clear(); + collision_check_object_debug_lifetime = 0.0; + } +}; +} // namespace behavior_path_planner::data::lane_change + +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index d6deecd4f46fa..427a26e9b4295 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -15,9 +15,11 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include @@ -26,6 +28,7 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; +using behavior_path_planner::data::lane_change::Debug; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( @@ -37,5 +40,6 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & current_lane_objects, const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); +MarkerArray createDebugMarkerArray(const Debug & debug_data); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 9e42b49635b82..3af4f487a0fa0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -45,7 +45,7 @@ struct LaneChangeStatus std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; - bool is_valid_path{true}; + bool is_valid_path{false}; double start_distance{0.0}; }; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 67506326d92aa..e16afcdbf19ec 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -115,6 +115,10 @@ ShiftLine getLaneChangingShiftLine( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & reference_path, const double shift_length); +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length); + PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 448f83ecaf15e..e3763720c2d8a 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -50,9 +50,6 @@ LaneChangeInterface::LaneChangeInterface( void LaneChangeInterface::processOnEntry() { waitApproval(); - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); } void LaneChangeInterface::processOnExit() @@ -73,13 +70,21 @@ bool LaneChangeInterface::isExecutionRequested() const bool LaneChangeInterface::isExecutionReady() const { - return module_type_->isSafe(); + return module_type_->isSafe() && !module_type_->isAbortState(); } void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); + + if (isWaitingApproval()) { + module_type_->updateLaneChangeStatus(); + } + updateDebugMarker(); + module_type_->updateSpecialData(); module_type_->resetStopPose(); } @@ -98,21 +103,29 @@ BehaviorModuleOutput LaneChangeInterface::plan() return {}; } - module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); - module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; stop_pose_ = module_type_->getStopPose(); - for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } updateSteeringFactorPtr(output); - clearWaitingApproval(); + if (module_type_->isAbortState()) { + waitApproval(); + removeRTCStatus(); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + } else { + clearWaitingApproval(); + } return output; } @@ -120,30 +133,20 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { *prev_approved_path_ = getPreviousModuleOutput().path; - module_type_->insertStopPoint( - module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = *prev_approved_path_; - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); - setObjectDebugVisualization(); + out = module_type_->getTerminalLaneChangePath(); + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + out.turn_signal_info = + getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info); - for (const auto & [uuid, data] : module_type_->getDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - // change turn signal when the vehicle reaches at the end of the path for waiting lane change - out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { @@ -211,6 +214,7 @@ bool LaneChangeInterface::canTransitSuccessState() } if (module_type_->hasFinishedLaneChange()) { + module_type_->resetParameters(); log_debug_throttled("Lane change process has completed."); return true; } @@ -293,66 +297,15 @@ bool LaneChangeInterface::canTransitFailureState() return false; } -bool LaneChangeInterface::canTransitIdleToRunningState() +void LaneChangeInterface::updateDebugMarker() const { - setObjectDebugVisualization(); - - auto log_debug_throttled = [&](std::string_view message) -> void { - RCLCPP_DEBUG(getLogger(), "%s", message.data()); - }; - - log_debug_throttled(__func__); - - if (!isActivated() || isWaitingApproval()) { - if (module_type_->specialRequiredCheck()) { - return true; - } - log_debug_throttled("Module is idling."); - return false; - } - - log_debug_throttled("Can lane change safely. Executing lane change."); - module_type_->toNormalState(); - return true; -} - -void LaneChangeInterface::setObjectDebugVisualization() const -{ - debug_marker_.markers.clear(); if (!parameters_->publish_debug_marker) { return; } - using marker_utils::showPolygon; - using marker_utils::showPredictedPath; - using marker_utils::showSafetyCheckInfo; - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showFilteredObjects; - - const auto debug_data = module_type_->getDebugData(); - const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); - const auto debug_valid_path = module_type_->getDebugValidPath(); - const auto debug_filtered_objects = module_type_->getDebugFilteredObjects(); debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); - add(showFilteredObjects( - debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, - debug_filtered_objects.other_lane, "object_filtered")); - if (!debug_data.empty()) { - add(showSafetyCheckInfo(debug_data, "object_debug_info")); - add(showPredictedPath(debug_data, "ego_predicted_path")); - add(showPolygon(debug_data, "ego_and_target_polygon_relation")); - } - - if (!debug_after_approval.empty()) { - add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); - add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); - } + using marker_utils::lane_change_markers::createDebugMarkerArray; + debug_marker_ = createDebugMarkerArray(module_type_->getDebugData()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() @@ -468,16 +421,13 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; const double path_length = motion_utils::calcArcLength(path.points); - const auto & front_point = path.points.front().point.pose.position; const size_t & current_nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double length_front_to_ego = motion_utils::calcSignedArcLength( - path.points, front_point, static_cast(0), current_pose.position, - current_nearest_seg_idx); + const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); - if (path_length - length_front_to_ego < buffer && start_pose) { + if (dist_to_terminal - base_to_front < buffer && start_pose) { // modify turn signal current_turn_signal_info.desired_start_point = *start_pose; current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c7ddd8fe63636..a5c49c32e0eb6 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -97,7 +97,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_parameters_->rss_params_for_stuck, is_stuck); } - debug_valid_path_ = valid_paths; + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; @@ -115,17 +115,17 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } -bool NormalLaneChange::isLaneChangeRequired() const +bool NormalLaneChange::isLaneChangeRequired() { - const auto current_lanes = getCurrentLanes(); + status_.current_lanes = getCurrentLanes(); - if (current_lanes.empty()) { + if (status_.current_lanes.empty()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); - return !target_lanes.empty(); + return !status_.target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -140,6 +140,59 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + BehaviorModuleOutput output; + + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = prev_turn_signal_info_; + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; + } + + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + const auto terminal_path = + calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + if (!terminal_path) { + RCLCPP_DEBUG(logger_, "Terminal path not found!!!"); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + output.path = terminal_path->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = updateOutputTurnSignal(); + + extendOutputDrivableArea(output); + + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output; +} + BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; @@ -183,7 +236,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() return output; } -void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -420,11 +473,13 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; + status_ = {}; + lane_change_debug_.reset(); - object_debug_.clear(); + RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } -TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() +TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( @@ -833,6 +888,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); // Guard if (objects.objects.empty()) { @@ -902,6 +961,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }; + // ignore object that are ahead of terminal lane change start + { + double distance_to_terminal_from_object = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon_outer) { + const auto obj_p = tier4_autoware_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 double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); + if (dist < distance_to_terminal_from_object) { + distance_to_terminal_from_object = dist; + } + } + if (minimum_lane_change_length > distance_to_terminal_from_object) { + continue; + } + } + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -1105,7 +1182,7 @@ bool NormalLaneChange::getLaneChangePaths( const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const { - object_debug_.clear(); + lane_change_debug_.collision_check_objects.clear(); if (current_lanes.empty() || target_lanes.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; @@ -1151,7 +1228,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1378,9 +1455,10 @@ bool NormalLaneChange::getLaneChangePaths( std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + !is_stuck && + utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route, + *lane_change_parameters_, lane_change_debug_.collision_check_objects)) { debug_print( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1393,7 +1471,8 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, rss_params, is_stuck, object_debug_); + *candidate_path, target_objects, rss_params, is_stuck, + lane_change_debug_.collision_check_objects); if (is_safe) { debug_print("ACCEPT!!!: it is valid and safe!"); @@ -1409,6 +1488,151 @@ bool NormalLaneChange::getLaneChangePaths( return false; } +std::optional NormalLaneChange::calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + + // lane changing start getEgoPose() is at the end of prepare segment + const auto current_lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); + + double distance_to_terminal_from_goal = 0; + if (is_goal_in_route) { + distance_to_terminal_from_goal = + utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); + } + + const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + prev_module_path_.points, current_lane_terminal_point, + -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + + if (!lane_changing_start_pose) { + RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); + return {}; + } + + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose.value()); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); + return {}; + } + + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( + target_lanes, lane_changing_start_pose.value()); + + const auto [min_lateral_acc, max_lateral_acc] = + lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, + minimum_lane_changing_velocity, next_lane_change_buffer); + + if (target_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); + return {}; + } + + const lanelet::BasicPoint2d lc_start_point( + lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); + + const auto target_lane_polygon = + lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; + lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = lane_changing_start_pose.value(); + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = max_lateral_acc; + lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; + + if (!is_valid_start_point) { + RCLCPP_DEBUG( + logger_, + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + return {}; + } + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_change_buffer, minimum_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, + lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); + + if (target_lane_reference_path.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); + return {}; + } + + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_changing_start_pose.value(), target_segment.points.front().point.pose, + target_lane_reference_path, shift_length); + + auto reference_segment = prev_module_path_; + const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + reference_segment.points, reference_segment.points.front().point.pose.position, + lane_changing_start_pose->position); + utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + // remove terminal points because utils::clipPathLength() calculates extra long path + reference_segment.points.pop_back(); + reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + + const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( + lane_change_info, reference_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + return terminal_lane_change_path; +} + PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1416,7 +1640,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto & target_lanes = status_.target_lanes; const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); @@ -1424,23 +1648,24 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose - object_debug_.clear(); - object_debug_lifetime_ += (stop_watch_.toc(getModuleTypeStr()) / 1000); - if (object_debug_lifetime_ > 2.0) { + lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.collision_check_object_debug_lifetime += + (stop_watch_.toc(getModuleTypeStr()) / 1000); + if (lane_change_debug_.collision_check_object_debug_lifetime > 2.0) { stop_watch_.toc(getModuleTypeStr(), true); - object_debug_lifetime_ = 0.0; - object_debug_after_approval_.clear(); + lane_change_debug_.collision_check_object_debug_lifetime = 0.0; + lane_change_debug_.collision_check_objects_after_approval.clear(); } if (!safety_status.is_safe) { - object_debug_after_approval_ = debug_data; + lane_change_debug_.collision_check_objects_after_approval = debug_data; } } return safety_status; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() +TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const { const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { double accumulated_length = 0.0; @@ -1460,6 +1685,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() TurnSignalInfo turn_signal_info{}; + if (path.path.points.empty()) { + return turn_signal_info; + } + // desired start pose = prepare start pose turn_signal_info.desired_start_point = std::invoke([&]() { const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; @@ -1639,31 +1868,25 @@ bool NormalLaneChange::calcAbortPath() ShiftedPath shifted_path; // offset front side - // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } - const auto arc_position = lanelet::utils::getArcCoordinates( - reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); - const PathWithLaneId reference_lane_segment = std::invoke([&]() { - const double s_start = arc_position.length; - double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); - - if ( - !reference_lanelets.empty() && - route_handler->isInGoalRouteSection(reference_lanelets.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = std::max(goal_arc_coordinates.length, s_start); - s_end = std::min(s_end, forward_length); + PathWithLaneId reference_lane_segment = prev_module_path_; + { + const auto terminal_path = + calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + if (terminal_path) { + reference_lane_segment = terminal_path->path; } - PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); - ref.points.back().point.longitudinal_velocity_mps = std::min( - ref.points.back().point.longitudinal_velocity_mps, - static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); - return ref; - }); + const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; + const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, + common_param.ego_nearest_yaw_threshold); + reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points, return_pose.position, seg_idx, + common_param.forward_path_length, 0.0); + } auto abort_path = selected_path; abort_path.shifted_path = shifted_path; @@ -1795,7 +2018,7 @@ bool NormalLaneChange::isVehicleStuck( using lanelet::utils::getArcCoordinates; const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; - for (const auto & object : debug_filtered_objects_.current_lane) { + for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point // Note: it needs chattering prevention. diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 719acba405a68..523b770734bc5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -120,4 +121,47 @@ MarkerArray showFilteredObjects( marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); return marker_array; } + +MarkerArray createDebugMarkerArray(const Debug & debug_data) +{ + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; + using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showFilteredObjects; + + const auto & debug_collision_check_object = debug_data.collision_check_objects; + const auto & debug_collision_check_object_after_approval = + debug_data.collision_check_objects_after_approval; + const auto & debug_valid_paths = debug_data.valid_paths; + const auto & debug_filtered_objects = debug_data.filtered_objects; + + MarkerArray debug_marker; + const auto add = [&debug_marker](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker); + }; + + add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); + add(showFilteredObjects( + debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, + debug_filtered_objects.other_lane, "object_filtered")); + + if (!debug_collision_check_object.empty()) { + add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info")); + add(showPredictedPath(debug_collision_check_object, "ego_predicted_path")); + add(showPolygon(debug_collision_check_object, "ego_and_target_polygon_relation")); + } + + if (!debug_collision_check_object_after_approval.empty()) { + add(showSafetyCheckInfo( + debug_collision_check_object_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath( + debug_collision_check_object_after_approval, "ego_predicted_path_after_approval")); + add(showPolygon( + debug_collision_check_object_after_approval, + "ego_and_target_polygon_relation_after_approval")); + } + + return debug_marker; +} } // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index df73990b770a3..e6a162d2bdad5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -412,10 +412,6 @@ PathWithLaneId getReferencePathFromTargetLane( return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); }); - if (s_end - s_start < lane_changing_length) { - return PathWithLaneId(); - } - RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") @@ -423,6 +419,10 @@ PathWithLaneId getReferencePathFromTargetLane( .get_child("getReferencePathFromTargetLane"), "start: %f, end: %f", s_start, s_end); + if (s_end - s_start < lane_changing_length) { + return PathWithLaneId(); + } + const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); @@ -437,6 +437,14 @@ ShiftLine getLaneChangingShiftLine( const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; + return getLaneChangingShiftLine( + lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); +} + +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ ShiftLine shift_line; shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose; diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index 439e08fd7e94f..c2b6babc402a2 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -82,3 +82,12 @@ keep_last: false priority: 7 max_module_size: 1 + + sampling_planner: + enable_module: true + enable_rtc: false + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: false + keep_last: true + priority: 16 + max_module_size: 1 diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml index 19ea0491e776f..79c590363beb5 100644 --- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml +++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml @@ -19,6 +19,7 @@ + @@ -49,6 +50,7 @@ + diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 2bc73325f96d5..7f50c61a8343a 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -43,6 +43,7 @@ autoware_perception_msgs behavior_path_planner_common freespace_planning_algorithms + frenet_planner geometry_msgs interpolation lane_departure_checker @@ -52,6 +53,7 @@ magic_enum motion_utils object_recognition_utils + path_sampler planning_test_utils pluginlib rclcpp diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index cbcec2e3095d3..c60bc53a1129e 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -226,8 +226,7 @@ void PlannerManager::generateCombinedDrivableArea( } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, - is_driving_forward); + output.path, di.drivable_lanes, false, false, false, data, is_driving_forward); } else { const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); @@ -239,7 +238,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, + di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data, is_driving_forward); } @@ -630,32 +629,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast(); + const auto get_sorted_keep_last_modules = [this](const auto & modules) { + std::vector keep_last_modules; + + std::copy_if( + modules.begin(), modules.end(), std::back_inserter(keep_last_modules), + [this](const auto & m) { return getManager(m)->isKeepLast(); }); + + // sort by priority (low -> high) + std::sort( + keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) { + return getManager(a)->getPriority() < getManager(b)->getPriority(); + }); + + return keep_last_modules; }; - move_to_end(approved_module_ptrs_, keep_last_module_cond); + + for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) { + move_to_end(approved_module_ptrs_, module); + } } // lock approved modules besides last one @@ -768,6 +773,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() == ModuleStatus::SUCCESS; }; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 31dc117fbce35..28943b5724fe8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -106,6 +106,7 @@ struct DrivableAreaInfo std::vector obstacles{}; // obstacles to extract from the drivable area bool enable_expanding_hatched_road_markings{false}; bool enable_expanding_intersection_areas{false}; + bool enable_expanding_freespace_areas{false}; // temporary only for pull over's freespace planning double drivable_margin{0.0}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 1a8c8241abe1a..73f7448133ee7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -373,13 +373,10 @@ class SceneModuleInterface RCLCPP_DEBUG(getLogger(), "%s", message.data()); }; if (current_state_ == ModuleStatus::IDLE) { - if (canTransitIdleToRunningState()) { - log_debug_throttled("transiting from IDLE to RUNNING"); - return ModuleStatus::RUNNING; - } - - log_debug_throttled("transiting from IDLE to IDLE"); - return ModuleStatus::IDLE; + auto init_state = setInitState(); + RCLCPP_DEBUG( + getLogger(), "transiting from IDLE to %s", magic_enum::enum_name(init_state).data()); + return init_state; } if (current_state_ == ModuleStatus::RUNNING) { @@ -460,9 +457,9 @@ class SceneModuleInterface virtual bool canTransitFailureState() = 0; /** - * @brief State transition condition IDLE -> RUNNING + * @brief Explicitly set the initial state */ - virtual bool canTransitIdleToRunningState() = 0; + virtual ModuleStatus setInitState() const { return ModuleStatus::RUNNING; } /** * @brief Get candidate path. This information is used for external judgement. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 9053da45708a0..9afa2608e1db6 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -19,6 +19,7 @@ #include #include +#include #include namespace behavior_path_planner::utils { @@ -40,8 +41,8 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward = true); + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward = true); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, @@ -62,6 +63,11 @@ std::vector expandLanelets( void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left); + std::vector getBoundWithHatchedRoadMarkings( const std::vector & original_bound, const std::shared_ptr & route_handler); @@ -72,14 +78,22 @@ std::vector getBoundWithIntersectionAreas( const std::vector & drivable_lanes, const bool is_left); std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool enable_expanding_freespace_areas, const bool is_left, + const bool is_driving_forward = true); + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left); + const bool is_left, const bool is_driving_forward = true); -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left); +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left); DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 03fae6864fe50..18627e8396239 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -154,6 +154,7 @@ std::vector extractBoundFromPolygon( const bool clockwise) { std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { ret.push_back(polygon[i]); @@ -765,54 +766,27 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward) + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward) { - // extract data - const auto transformed_lanes = utils::transformToLanelets(lanes); - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto route_handler = planner_data->route_handler; - constexpr double overlap_threshold = 0.01; - if (path.points.empty()) { return; } - auto addPoints = - [](const lanelet::ConstLineString3d & points, std::vector & bound) { - for (const auto & bound_p : points) { - const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if (bound.empty()) { - bound.push_back(cp); - } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { - bound.push_back(cp); - } - } - }; - - const auto has_overlap = - [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { - for (const auto & transformed_lane : transformed_lanes) { - if (checkHasSameLane(ignore_lanelets, transformed_lane)) { - continue; - } - if (boost::geometry::intersects( - lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; + path.left_bound.clear(); + path.right_bound.clear(); // Insert Position - auto left_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, true); - auto right_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, false); - - if (left_bound.empty() || right_bound.empty()) { + path.left_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, true, + is_driving_forward); + path.right_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, false, + is_driving_forward); + + if (path.left_bound.empty() || path.right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, @@ -820,135 +794,10 @@ void generateDrivableArea( return; } - // Insert points after goal - lanelet::ConstLanelet goal_lanelet; - if ( - route_handler->getGoalLanelet(&goal_lanelet) && - checkHasSameLane(transformed_lanes, goal_lanelet)) { - const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); - const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); - const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); - const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); - lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; - if (goal_left_lanelet) { - goal_lanelets.push_back(*goal_left_lanelet); - } - if (goal_right_lanelet) { - goal_lanelets.push_back(*goal_right_lanelet); - } - - for (const auto & lane : lanes_after_goal) { - // If lane is already in the transformed lanes, ignore it - if (checkHasSameLane(transformed_lanes, lane)) { - continue; - } - // Check if overlapped - const bool is_overlapped = - (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) - : has_overlap(lane)); - if (is_overlapped) { - continue; - } - - addPoints(lane.leftBound3d(), left_bound); - addPoints(lane.rightBound3d(), right_bound); - } - } - - if (!is_driving_forward) { - std::reverse(left_bound.begin(), left_bound.end()); - std::reverse(right_bound.begin(), right_bound.end()); - } - - path.left_bound.clear(); - path.right_bound.clear(); - - const auto [left_start_idx, right_start_idx] = [&]() { - const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); - const auto cropped_path_points = motion_utils::cropPoints( - path.points, current_pose.position, current_seg_idx, - planner_data->parameters.forward_path_length, - planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); - - constexpr double front_length = 0.5; - const auto front_pose = - cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; - const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose, M_PI_2); - const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose, M_PI_2); - const auto left_start_point = - calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_point = calcLongitudinalOffsetStartPoint( - right_bound, front_pose, front_right_start_idx, -front_length); - const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); - const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); - - // Insert a start point - path.left_bound.push_back(left_start_point); - path.right_bound.push_back(right_start_point); - - return std::make_pair(left_start_idx, right_start_idx); - }(); - - // Get Closest segment for the goal point - const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; - const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose, M_PI_2); - const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose, M_PI_2); - const auto left_goal_point = - calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_point = - calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); - const size_t left_goal_idx = std::max( - goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); - const size_t right_goal_idx = std::max( - goal_right_start_idx, - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); - - // Insert middle points - for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); - if (dist > overlap_threshold) { - path.left_bound.push_back(next_point); - } - } - for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); - if (dist > overlap_threshold) { - path.right_bound.push_back(next_point); - } - } - - // Insert a goal point - if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > - overlap_threshold) { - path.left_bound.push_back(left_goal_point); - } - if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > - overlap_threshold) { - path.right_bound.push_back(right_goal_point); - } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { drivable_area_expansion::expand_drivable_area(path, planner_data); } - - // make bound longitudinally monotonic - // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if ( - is_driving_forward && - (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) { - makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound - makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound - } } void generateDrivableArea( @@ -1400,15 +1249,333 @@ std::vector getBoundWithIntersectionAreas( return expanded_bound; } +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left) +{ + using lanelet::utils::to2D; + using lanelet::utils::conversion::toGeomMsgPt; + using lanelet::utils::conversion::toLaneletPoint; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto & route_handler = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return std::make_pair(original_bound, false); + } + + std::sort(polygons.begin(), polygons.end(), [&ego_pose](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + to2D(a).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + const double b_distance = boost::geometry::distance( + to2D(b).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + return a_distance < b_distance; + }); + + const auto & polygon = polygons.front(); + + const auto original_bound_itr = + std::find_if(original_bound.begin(), original_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto original_bound_rev_itr = + std::find_if(original_bound.rbegin(), original_bound.rend(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto other_side_bound_itr = + std::find_if(other_side_bound.begin(), other_side_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + if ( + original_bound_itr == original_bound.end() || other_side_bound_itr == other_side_bound.end()) { + return std::make_pair(original_bound, false); + } + + const auto footprint = planner_data->parameters.vehicle_info.createFootprint(); + const auto vehicle_polygon = transformVector(footprint, pose2transform(ego_pose)); + const auto is_driving_freespace = + !boost::geometry::disjoint(vehicle_polygon, to2D(polygon).basicPolygon()); + + const auto is_clockwise_polygon = boost::geometry::is_valid(to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + + const auto extract_bound_from_polygon = [&polygon, &is_clockwise_iteration]( + const auto & start_id, const auto & end_id) { + const auto start_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == start_id; }); + + const auto end_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == end_id; }); + + // extract line strings between start_idx and end_idx. + const size_t start_idx = std::distance(polygon.begin(), start_point_itr); + const size_t end_idx = std::distance(polygon.begin(), end_point_itr); + + return extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); + }; + + const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left]( + const auto & bound, const auto trim_behind_bound) { + if (!is_driving_freespace) { + return bound; + } + + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); + + std::vector ret; + for (size_t i = 1; i < bound.size(); ++i) { + const auto intersect = tier4_autoware_utils::intersect( + ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), + toGeomMsgPt(bound.at(i))); + + ret.push_back(bound.at(i - 1)); + + if (intersect.has_value()) { + ret.emplace_back( + lanelet::InvalId, intersect.value().x, intersect.value().y, intersect.value().z); + break; + } + } + + return ret; + }; + + std::vector expanded_bound; + + enum class RouteCase { + ROUTE_IS_PASS_THROUGH_FREESPACE = 0, + GOAL_POS_IS_IN_FREESPACE, + INIT_POS_IS_IN_FREESPACE, + OTHER, + }; + + const auto route_case = [&]() { + if (original_bound_itr->id() != original_bound_rev_itr->id()) { + return RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.front().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::INIT_POS_IS_IN_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.back().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::GOAL_POS_IS_IN_FREESPACE; + } + return RouteCase::OTHER; + }(); + + switch (route_case) { + case RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), original_bound_rev_itr->id()); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), polygon_bound.begin(), polygon_bound.end()); + expanded_bound.insert( + expanded_bound.end(), std::next(original_bound_rev_itr).base(), original_bound.end()); + break; + } + case RouteCase::INIT_POS_IS_IN_FREESPACE: { + auto polygon_bound = + extract_bound_from_polygon(other_side_bound_itr->id(), original_bound_itr->id()); + std::reverse(polygon_bound.begin(), polygon_bound.end()); + auto bound_edge = get_bound_edge(polygon_bound, true); + std::reverse(bound_edge.begin(), bound_edge.end()); + + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + expanded_bound.insert(expanded_bound.end(), original_bound_itr, original_bound.end()); + break; + } + case RouteCase::GOAL_POS_IS_IN_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), other_side_bound_itr->id()); + const auto bound_edge = get_bound_edge(polygon_bound, false); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + break; + } + case RouteCase::OTHER: { + expanded_bound = original_bound; + break; + } + default: + throw std::domain_error("invalid case."); + } + + const auto goal_is_in_freespace = boost::geometry::within( + to2D(toLaneletPoint(route_handler->getGoalPose().position).basicPoint()), + to2D(polygon).basicPolygon()); + + return std::make_pair(expanded_bound, is_driving_freespace || goal_is_in_freespace); +} + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left, const bool is_driving_forward) +{ + const auto lanelets = utils::transformToLanelets(drivable_lanes); + const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & route_handler = planner_data->route_handler; + const auto & vehicle_length = planner_data->parameters.vehicle_length; + constexpr double overlap_threshold = 0.01; + + const auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } + } + }; + + const auto has_overlap = + [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { + for (const auto & transformed_lane : lanelets) { + if (checkHasSameLane(ignore_lanelets, transformed_lane)) { + continue; + } + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; + + std::vector processed_bound; + std::vector tmp_bound = original_bound; + + // Insert points after goal + lanelet::ConstLanelet goal_lanelet; + if (route_handler->getGoalLanelet(&goal_lanelet) && checkHasSameLane(lanelets, goal_lanelet)) { + const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); + const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); + lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; + if (goal_left_lanelet.has_value()) { + goal_lanelets.push_back(goal_left_lanelet.value()); + } + if (goal_right_lanelet.has_value()) { + goal_lanelets.push_back(goal_right_lanelet.value()); + } + + for (const auto & lane : lanes_after_goal) { + // If lane is already in the transformed lanes, ignore it + if (checkHasSameLane(lanelets, lane)) { + continue; + } + // Check if overlapped + const bool is_overlapped = + (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) + : has_overlap(lane)); + if (is_overlapped) { + continue; + } + + if (is_left) { + addPoints(lane.leftBound3d(), tmp_bound); + } else { + addPoints(lane.rightBound3d(), tmp_bound); + } + } + } + + if (!is_driving_forward) { + std::reverse(tmp_bound.begin(), tmp_bound.end()); + } + + const auto start_idx = [&]() { + const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); + const auto cropped_path_points = motion_utils::cropPoints( + path.points, current_pose.position, current_seg_idx, + planner_data->parameters.forward_path_length, + planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); + + constexpr double front_length = 0.5; + const auto front_pose = + cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; + const size_t front_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, front_pose, M_PI_2); + const auto start_point = + calcLongitudinalOffsetStartPoint(tmp_bound, front_pose, front_start_idx, -front_length); + + // Insert a start point + processed_bound.push_back(start_point); + + return findNearestSegmentIndexFromLateralDistance(tmp_bound, start_point); + }(); + + // Get Closest segment for the goal point + const auto [goal_idx, goal_point] = [&]() { + const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; + const size_t goal_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2); + const auto goal_point = + calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length); + const size_t goal_idx = + std::max(goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_point)); + + return std::make_pair(goal_idx, goal_point); + }(); + + // Insert middle points + for (size_t i = start_idx + 1; i <= goal_idx; ++i) { + const auto & next_point = tmp_bound.at(i); + const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); + if (dist > overlap_threshold) { + processed_bound.push_back(next_point); + } + } + + // Insert a goal point + if ( + tier4_autoware_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { + processed_bound.push_back(goal_point); + } + + const bool skip_monotonic_process = + !is_driving_forward || + !(enable_expanding_hatched_road_markings || enable_expanding_intersection_areas); + + return skip_monotonic_process + ? processed_bound + : makeBoundLongitudinallyMonotonic(processed_bound, path, planner_data, is_left); +} + // calculate bounds from drivable lanes and hatched road markings std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) + const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) { + using motion_utils::removeOverlapPoints; + + const auto & route_handler = planner_data->route_handler; + // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { + const auto convert_to_points = [](const auto & drivable_lanes, const auto is_left) { constexpr double overlap_threshold = 0.01; std::vector points; @@ -1435,20 +1602,35 @@ std::vector calcBound( }; // Step1. create drivable bound from drivable lanes. - std::vector bound_points = convert_to_points(drivable_lanes); + auto [bound_points, skip_post_process] = [&]() { + if (!enable_expanding_freespace_areas) { + return std::make_pair(convert_to_points(drivable_lanes, is_left), false); + } + return getBoundWithFreeSpaceAreas( + convert_to_points(drivable_lanes, is_left), convert_to_points(drivable_lanes, !is_left), + planner_data, is_left); + }(); + + const auto post_process = [&](const auto & bound, const auto skip) { + return skip + ? bound + : postProcess( + bound, path, planner_data, drivable_lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, is_left, is_driving_forward); + }; // Step2. if there is no drivable area defined by polygon, return original drivable bound. if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } - // Step3. if there are hatched road markings, expand drivable bound with the polygon. + // Step3.if there are hatched road markings, expand drivable bound with the polygon. if (enable_expanding_hatched_road_markings) { bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); } if (!enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } // Step4. if there are intersection areas, expand drivable bound with the polygon. @@ -1457,12 +1639,12 @@ std::vector calcBound( getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); } - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left) +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left) { using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcAzimuthAngle; @@ -1508,7 +1690,7 @@ void makeBoundLongitudinallyMonotonic( const auto get_bound_with_pose = [&](const auto & bound_with_pose, const auto & path_points) { auto ret = bound_with_pose; - const double offset = is_bound_left ? 100.0 : -100.0; + const double offset = is_left ? 100.0 : -100.0; size_t start_bound_idx = 0; @@ -1588,14 +1770,14 @@ void makeBoundLongitudinallyMonotonic( // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is // opposite. - const double lat_offset = is_bound_left ? 100.0 : -100.0; + const double lat_offset = is_left ? 100.0 : -100.0; const auto p_bound_1 = getPose(bound_with_pose.at(bound_idx)); const auto p_bound_2 = getPose(bound_with_pose.at(bound_idx + 1)); const auto p_bound_offset = calcOffsetPose(p_bound_1, 0.0, lat_offset, 0.0); - if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_bound_left, is_reverse)) { + if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_left, is_reverse)) { bound_idx++; continue; } @@ -1681,18 +1863,16 @@ void makeBoundLongitudinallyMonotonic( return ret; }; - const auto original_bound = is_bound_left ? path.left_bound : path.right_bound; - if (path.points.empty()) { - return; + return original_bound; } - if (original_bound.empty()) { - return; + if (original_bound.size() < 2) { + return original_bound; } if (path.points.front().lane_ids.empty()) { - return; + return original_bound; } // step.1 create bound with pose vector. @@ -1734,14 +1914,14 @@ void makeBoundLongitudinallyMonotonic( p.forward_path_length, p); if (centerline_path.points.size() < 2) { - return; + return original_bound; } const auto ego_idx = planner_data->findEgoIndex(centerline_path.points); const auto end_idx = findNearestSegmentIndex(centerline_path.points, original_bound.back()); if (ego_idx >= end_idx) { - return; + return original_bound; } clipped_points.insert( @@ -1750,7 +1930,7 @@ void makeBoundLongitudinallyMonotonic( } if (clipped_points.empty()) { - return; + return original_bound; } // step.3 update bound pose by reference path pose. @@ -1771,11 +1951,7 @@ void makeBoundLongitudinallyMonotonic( auto full_monotonic_bound = remove_orientation(full_monotonic_bound_with_pose); // step.7 remove sharp bound points. - if (is_bound_left) { - path.left_bound = remove_sharp_points(full_monotonic_bound); - } else { - path.right_bound = remove_sharp_points(full_monotonic_bound); - } + return remove_sharp_points(full_monotonic_bound); } std::vector combineDrivableLanes( @@ -1785,7 +1961,9 @@ std::vector combineDrivableLanes( const auto has_same_lane = [](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) { return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) { - return ll.id() == target_lane.id(); + return ll.id() == target_lane.id() && + ll.rightBound3d().id() == target_lane.rightBound3d().id() && + ll.leftBound3d().id() == target_lane.leftBound3d().id(); }) != lanes.end(); }; @@ -1884,6 +2062,15 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // enable expanding freespace areas + combined_drivable_area_info.enable_expanding_freespace_areas = + drivable_area_info1.enable_expanding_freespace_areas || + drivable_area_info2.enable_expanding_freespace_areas; + + // drivable margin + combined_drivable_area_info.drivable_margin = + std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); + return combined_drivable_area_info; } diff --git a/planning/behavior_path_sampling_planner_module/CMakeLists.txt b/planning/behavior_path_sampling_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..4d2198e7748c7 --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_sampling_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/sampling_planner_module.cpp + src/manager.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_sampling_planner_module/README.md b/planning/behavior_path_sampling_planner_module/README.md new file mode 100644 index 0000000000000..601c5b4b26d01 --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/README.md @@ -0,0 +1,78 @@ +# Behavior Path Sampling Based Planner + +WARNING: This module is experimental and has not been properly tested on a real vehicle, use only on simulations. + +## Purpose + +This package implements a node that uses sampling based planning to generate a drivable trajectory for the behavior path planner. It is heavily based off the [sampling_based_planner module](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/sampling_based_planner). + +## Features + +This package is able to: + +- create a smooth trajectory to avoid static obstacles. +- guarantees the generated trajectory (if any) complies with customizable hard constraints. +- transitions to a success state after the ego vehicle merges to its goal lane. +- re-use previously generated outputs to re-sample new alternative paths + +Note that the velocity is just taken over from the input path. + +## Algorithm + +Sampling based planning is decomposed into 3 successive steps: + +1. Sampling: candidate trajectories are generated. +2. Pruning: invalid candidates are discarded. +3. Selection: the best remaining valid candidate is selected. + +### Sampling + +Candidate trajectories are generated based on the current ego state and some target state. +1 sampling algorithms is currently implemented: polynomials in the frenet frame. + +### Pruning + +The validity of each candidate trajectory is checked using a set of hard constraints. + +- collision: ensure no collision with static obstacles; +- curvature: ensure smooth curvature; +- drivable area: ensure the trajectory stays within the drivable area. + +### Selection + +Among the valid candidate trajectories, the _best_ one is determined using a set of soft constraints (i.e., objective functions). + +- curvature: prefer smoother trajectories; +- length: prefer trajectories with longer remaining path length; +- lateral deviation: prefer trajectories close to the goal. + +Each soft constraint is associated with a weight to allow tuning of the preferences. + +## Limitations + +The quality of the candidates generated with polynomials in frenet frame greatly depend on the reference path. +If the reference path is not smooth, the resulting candidates will probably be un-drivable. + +Failure to find a valid trajectory current results in a suddenly stopping trajectory. + +The module has troubles generating paths that converge rapidly to the goal lanelet. Basically, after overcoming all obstacles, the module should prioritize paths that rapidly make the ego vehicle converge back to its goal lane (ie. paths with low average and final lateral deviation). However, this does not function properly at the moment. + +Detection of proper merging can be rough: Sometimes, the module when detects that the ego has converged on the goal lanelet and that there are no more obstacles, the planner transitions to the goal planner, but the transition is not very smooth and could cause discomfort for the user. + +## Future works + +Some possible improvements for this module include: + +-Implementing a dynamic weight tuning algorithm: Dynamically change weights depending on the scenario (ie. to prioritize more the paths with low curvature and low avg. lat. deviation after all obstacles have been avoided). + +-Implementing multi-objective optimization to improve computing time and possibly make a more dynamic soft constraints weight tuning. [Related publication](https://ieeexplore.ieee.org/abstract/document/10180226). + +-Implement bezier curves as another method to obtain samples, see the [sampling_based_planner module](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/sampling_based_planner). + +-Explore the possibility to replace several or other behavior path modules with the sampling based behavior path module. + +-Perform real-life tests of this module once it has matured and some of its limitations have been solved. + +## Other possibilities + +The module is currently aimed at creating paths for static obstacle avoidance. However, the nature of sampling planner allows this module to be expanded or repurposed to other tasks such as lane changes, dynamic avoidance and general reaching of a goal. It is possible, with a good dynamic/scenario dependant weight tuning to use the sampling planning approach as a replacement for the other behavior path modules, assuming good candidate pruning and good soft constraints weight tuning. diff --git a/planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml b/planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml new file mode 100644 index 0000000000000..53c531c4b04a4 --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + start_planner: + max_curvature: 0.1 + min_curvature: -0.1 + lateral_deviation_weight: 1.0 + length_weight: 1.0 + curvature_weight: 1.0 + enable_frenet: true + enable_bezier: false + resolution: 0.5 + previous_path_reuse_points_nb: 2 + nb_target_lateral_positions: {10.0, 20.0} + target_lengths: {-2.0, -1.0, 0.0, 1.0, 2.0} + target_lateral_positions: 2 + target_lateral_velocities: {-0.1, 0.0, 0.1} + target_lateral_accelerations: {0.0} + force_zero_deviation: true + force_zero_heading: true + smooth_reference: false diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp new file mode 100644 index 0000000000000..1fc1534d915d5 --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp @@ -0,0 +1,54 @@ +// 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 BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ + +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp" +#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" + +#include + +#include +#include +#include +#include + +namespace behavior_path_planner +{ + +class SamplingPlannerModuleManager : public SceneModuleManagerInterface +{ +public: + SamplingPlannerModuleManager() : SceneModuleManagerInterface{"sampling_planner"} {} + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override + { + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); + } + + void updateModuleParams( + [[maybe_unused]] const std::vector & parameters) override; + +private: + std::shared_ptr parameters_; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp new file mode 100644 index 0000000000000..f72be654d1e1e --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -0,0 +1,266 @@ +// 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 BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ + +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" +#include "behavior_path_sampling_planner_module/util.hpp" +#include "bezier_sampler/bezier_sampling.hpp" +#include "frenet_planner/frenet_planner.hpp" +#include "lanelet2_extension/utility/query.hpp" +#include "lanelet2_extension/utility/utilities.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sampler_common/constraints/footprint.hpp" +#include "sampler_common/constraints/hard_constraint.hpp" +#include "sampler_common/constraints/soft_constraint.hpp" +#include "sampler_common/structures.hpp" +#include "sampler_common/transform/spline_transform.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "tier4_planning_msgs/msg/lateral_offset.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +namespace behavior_path_planner +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +struct SamplingPlannerData +{ + // input + std::vector traj_points; // converted from the input path + std::vector left_bound; + std::vector right_bound; + + // ego + geometry_msgs::msg::Pose ego_pose; + double ego_vel{}; +}; + +struct SamplingPlannerDebugData +{ + std::vector sampled_candidates{}; + size_t previous_sampled_candidates_nb = 0UL; + std::vector obstacles{}; + std::vector footprints{}; +}; +class SamplingPlannerModule : public SceneModuleInterface +{ +public: + SamplingPlannerModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); + + bool isExecutionRequested() const override; + bool isExecutionReady() const override; + BehaviorModuleOutput plan() override; + CandidateOutput planCandidate() const override; + void updateData() override; + bool isRootLaneletToBeUpdated() const override + { + return getCurrentStatus() == ModuleStatus::SUCCESS; + } + + void updateModuleParams(const std::any & parameters) override + { + std::shared_ptr user_params_ = + std::any_cast>(parameters); + + // Constraints + internal_params_->constraints.hard.max_curvature = user_params_->max_curvature; + internal_params_->constraints.hard.min_curvature = user_params_->min_curvature; + internal_params_->constraints.soft.lateral_deviation_weight = + user_params_->lateral_deviation_weight; + internal_params_->constraints.soft.length_weight = user_params_->length_weight; + internal_params_->constraints.soft.curvature_weight = user_params_->curvature_weight; + internal_params_->constraints.soft.weights = user_params_->weights; + internal_params_->constraints.ego_footprint = vehicle_info_.createFootprint(0.25); + internal_params_->constraints.ego_width = vehicle_info_.vehicle_width_m; + internal_params_->constraints.ego_length = vehicle_info_.vehicle_length_m; + // Sampling + internal_params_->sampling.enable_frenet = user_params_->enable_frenet; + internal_params_->sampling.enable_bezier = user_params_->enable_bezier; + internal_params_->sampling.resolution = user_params_->resolution; + internal_params_->sampling.previous_path_reuse_points_nb = + user_params_->previous_path_reuse_points_nb; + internal_params_->sampling.target_lengths = user_params_->target_lengths; + internal_params_->sampling.target_lateral_positions = user_params_->target_lateral_positions; + internal_params_->sampling.nb_target_lateral_positions = + user_params_->nb_target_lateral_positions; + + internal_params_->sampling.frenet.target_lateral_velocities = + user_params_->target_lateral_velocities; + internal_params_->sampling.frenet.target_lateral_accelerations = + user_params_->target_lateral_accelerations; + + // Preprocessing + internal_params_->preprocessing.force_zero_deviation = user_params_->force_zero_deviation; + internal_params_->preprocessing.force_zero_heading = user_params_->force_zero_heading; + internal_params_->preprocessing.smooth_reference = user_params_->smooth_reference; + } + + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & visitor) const override + { + } + + SamplingPlannerDebugData debug_data_; + behavior_path_planner::HardConstraintsFunctionVector hard_constraints_; + behavior_path_planner::SoftConstraintsFunctionVector soft_constraints_; + +private: + SamplingPlannerData createPlannerData( + const PlanResult & path, const std::vector & left_bound, + const std::vector & right_bound) const; + + PlanResult generatePath(); + + bool canTransitSuccessState() override + { + std::vector drivable_lanes{}; + const auto & prev_module_path = + std::make_shared(getPreviousModuleOutput().path); + const auto prev_module_reference_path = + std::make_shared(getPreviousModuleOutput().reference_path); + + const auto & p = planner_data_->parameters; + const auto ego_pose = planner_data_->self_odometry->pose.pose; + const auto goal_pose = planner_data_->route_handler->getGoalPose(); + + lanelet::ConstLanelet current_lane; + + if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "failed to find closest lanelet within route!!!"); + return false; + } + const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence( + current_lane, ego_pose, p.backward_path_length, p.forward_path_length); + // expand drivable lanes + std::for_each( + current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) { + drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_)); + }); + + lanelet::ConstLanelets current_lanes; + + for (auto & d : drivable_lanes) { + current_lanes.push_back(d.right_lane); + current_lanes.push_back(d.left_lane); + current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end()); + } + lanelet::ConstLanelet closest_lanelet_to_ego; + lanelet::utils::query::getClosestLanelet(current_lanes, ego_pose, &closest_lanelet_to_ego); + lanelet::ConstLanelet closest_lanelet_to_goal; + lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal); + const bool ego_and_goal_on_same_lanelet = + closest_lanelet_to_goal.id() == closest_lanelet_to_ego.id(); + + if (!ego_and_goal_on_same_lanelet) return false; + + const auto ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose); + const auto goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose); + const double length_to_goal = std::abs(goal_arc.length - ego_arc.length); + + constexpr double epsilon = 1E-5; + if (length_to_goal < epsilon) return isReferencePathSafe(); + + const auto nearest_index = + motion_utils::findNearestIndex(prev_module_reference_path->points, ego_pose); + if (!nearest_index) return false; + auto toYaw = [](const geometry_msgs::msg::Quaternion & quat) -> double { + const auto rpy = tier4_autoware_utils::getRPY(quat); + return rpy.z; + }; + const auto quat = prev_module_reference_path->points[*nearest_index].point.pose.orientation; + const double ref_path_yaw = toYaw(quat); + const double ego_yaw = toYaw(ego_pose.orientation); + const double yaw_difference = std::abs(ego_yaw - ref_path_yaw); + + // TODO(Daniel) magic numbers + constexpr double threshold_lat_distance_for_merging = 0.15; + constexpr double threshold_yaw_difference_for_merging = M_PI / 72.0; // 2.5 degrees + const bool merged_back_to_path = + (std::abs(ego_arc.distance) < threshold_lat_distance_for_merging) && + (yaw_difference < threshold_yaw_difference_for_merging); + return isReferencePathSafe() && (merged_back_to_path); + } + + bool canTransitFailureState() override { return false; } + + bool isReferencePathSafe() const; + + void updateDebugMarkers(); + + void prepareConstraints( + sampler_common::Constraints & constraints, + const PredictedObjects::ConstSharedPtr & predicted_objects, + const std::vector & left_bound, + const std::vector & right_bound) const; + + frenet_planner::SamplingParameters prepareSamplingParameters( + const sampler_common::State & initial_state, + const sampler_common::transform::Spline2D & path_spline, + const SamplingPlannerInternalParameters & internal_params_); + + PathWithLaneId convertFrenetPathToPathWithLaneID( + const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const double path_z); + + // member + // std::shared_ptr params_; + std::shared_ptr internal_params_; + vehicle_info_util::VehicleInfo vehicle_info_{}; + std::optional prev_sampling_path_ = std::nullopt; + // move to utils + + void extendOutputDrivableArea( + BehaviorModuleOutput & output, std::vector & drivable_lanes); + bool isEndPointsConnected( + const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) const; + DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, + const std::shared_ptr & planner_data) const; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp new file mode 100644 index 0000000000000..6e00fefe89574 --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -0,0 +1,91 @@ +// 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 BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ + +#include "bezier_sampler/bezier_sampling.hpp" +#include "sampler_common/structures.hpp" + +#include +namespace behavior_path_planner +{ +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +struct SamplingPlannerParameters +{ // constraints.hard + double max_curvature; + double min_curvature; + // constraints.soft + double lateral_deviation_weight; + double length_weight; + double curvature_weight; + std::vector weights; + + // sampling + bool enable_frenet; + bool enable_bezier; + double resolution; + int previous_path_reuse_points_nb; + int nb_target_lateral_positions; + + std::vector target_lengths; + std::vector target_lateral_positions; + // frenet + std::vector target_lateral_velocities; + std::vector target_lateral_accelerations; + + bool force_zero_deviation; + bool force_zero_heading; + bool smooth_reference; +}; + +struct Preprocessing +{ + bool force_zero_deviation{}; + bool force_zero_heading{}; + bool smooth_reference{}; +}; + +struct Frenet +{ + std::vector target_lateral_velocities{}; + std::vector target_lateral_accelerations{}; +}; + +struct Sampling +{ + bool enable_frenet{}; + bool enable_bezier{}; + double resolution{}; + int previous_path_reuse_points_nb{}; + std::vector target_lengths{}; + std::vector target_lateral_positions{}; + int nb_target_lateral_positions{}; + Frenet frenet; + bezier_sampler::SamplingParameters bezier{}; +}; + +struct SamplingPlannerInternalParameters +{ + sampler_common::Constraints constraints; + Sampling sampling; + Preprocessing preprocessing{}; +}; +} // namespace behavior_path_planner +#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp new file mode 100644 index 0000000000000..ac8ba788cb9bd --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp @@ -0,0 +1,104 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ +#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ +#include "sampler_common/structures.hpp" +#include "sampler_common/transform/spline_transform.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +namespace behavior_path_planner +{ + +using geometry_msgs::msg::Pose; + +struct SoftConstraintsInputs +{ + Pose goal_pose; + Pose ego_pose; + lanelet::ArcCoordinates ego_arc; + lanelet::ArcCoordinates goal_arc; + lanelet::ConstLanelets closest_lanelets_to_goal; + behavior_path_planner::PlanResult reference_path; + behavior_path_planner::PlanResult prev_module_path; + std::optional prev_path; + lanelet::ConstLanelets current_lanes; +}; + +using SoftConstraintsFunctionVector = std::vector>; + +using HardConstraintsFunctionVector = std::vector>; + +inline std::vector evaluateSoftConstraints( + sampler_common::Path & path, const sampler_common::Constraints & constraints, + const SoftConstraintsFunctionVector & soft_constraints_functions, + const SoftConstraintsInputs & input_data) +{ + std::vector constraints_results; + for (const auto & f : soft_constraints_functions) { + const auto cost = f(path, constraints, input_data); + constraints_results.push_back(cost); + } + if (constraints.soft.weights.size() != constraints_results.size()) { + path.cost = std::accumulate(constraints_results.begin(), constraints_results.end(), 0.0); + return constraints_results; + } + + path.cost = std::inner_product( + constraints_results.begin(), constraints_results.end(), constraints.soft.weights.begin(), 0.0); + return constraints_results; +} + +inline std::vector evaluateHardConstraints( + sampler_common::Path & path, const sampler_common::Constraints & constraints, + const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints) +{ + std::vector constraints_results; + bool constraints_passed = true; + for (const auto & f : hard_constraints) { + const bool constraint_check = f(path, constraints, footprint); + constraints_passed &= constraint_check; + constraints_results.push_back(constraint_check); + } + + path.constraints_satisfied = constraints_passed; + return constraints_results; +} + +inline sampler_common::State getInitialState( + const geometry_msgs::msg::Pose & pose, + const sampler_common::transform::Spline2D & reference_spline) +{ + sampler_common::State initial_state; + Point2d initial_state_pose{pose.position.x, pose.position.y}; + const auto rpy = tier4_autoware_utils::getRPY(pose.orientation); + initial_state.pose = initial_state_pose; + initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y}); + initial_state.heading = rpy.z; + return initial_state; +} + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml new file mode 100644 index 0000000000000..cfac87b3c557f --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -0,0 +1,52 @@ + + + + behavior_path_sampling_planner_module + 0.1.0 + The behavior_path_sampling_planner_module package + + Daniel Sanchez + Maxime Clement + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + autoware_auto_vehicle_msgs + autoware_perception_msgs + behavior_path_planner_common + bezier_sampler + frenet_planner + geometry_msgs + interpolation + lanelet2_extension + motion_utils + path_sampler + planning_test_utils + pluginlib + rclcpp + rclcpp_components + route_handler + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_sampling_planner_module/plugins.xml b/planning/behavior_path_sampling_planner_module/plugins.xml new file mode 100644 index 0000000000000..04a8dbb3171c0 --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_sampling_planner_module/src/manager.cpp b/planning/behavior_path_sampling_planner_module/src/manager.cpp new file mode 100644 index 0000000000000..724b87c4752e5 --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/src/manager.cpp @@ -0,0 +1,142 @@ +// 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 "behavior_path_sampling_planner_module/manager.hpp" + +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ + +void SamplingPlannerModuleManager::init(rclcpp::Node * node) +{ + // init manager interface + initInterface(node, {""}); + + SamplingPlannerParameters p{}; + { + std::string ns{"constraints.hard"}; + p.max_curvature = node->declare_parameter(ns + ".max_curvature"); + p.min_curvature = node->declare_parameter(ns + ".min_curvature"); + ns = std::string{"constraints.soft"}; + p.lateral_deviation_weight = + node->declare_parameter(ns + ".lateral_deviation_weight"); // [[unused]] Delete? + p.length_weight = node->declare_parameter(ns + ".length_weight"); // [[unused]] Delete? + p.curvature_weight = + node->declare_parameter(ns + ".curvature_weight"); // [[unused]] Delete? + p.weights = node->declare_parameter>(ns + ".weights"); + } + { + std::string ns{"sampling"}; + p.enable_frenet = node->declare_parameter(ns + ".enable_frenet"); + p.enable_bezier = node->declare_parameter( + ns + ".enable_bezier"); // [[unused]] will be used in the future + p.resolution = node->declare_parameter(ns + ".resolution"); + p.previous_path_reuse_points_nb = + node->declare_parameter(ns + ".previous_path_reuse_points_nb"); + p.nb_target_lateral_positions = + node->declare_parameter(ns + ".nb_target_lateral_positions"); + p.target_lengths = node->declare_parameter>(ns + ".target_lengths"); + p.target_lateral_positions = + node->declare_parameter>(ns + ".target_lateral_positions"); + ns += ".frenet"; + p.target_lateral_velocities = + node->declare_parameter>(ns + ".target_lateral_velocities"); + p.target_lateral_accelerations = + node->declare_parameter>(ns + ".target_lateral_accelerations"); + } + { + std::string ns{"preprocessing"}; + p.force_zero_deviation = node->declare_parameter( + ns + ".force_zero_initial_deviation"); // [[unused]] will be used in the future + p.force_zero_heading = node->declare_parameter( + ns + ".force_zero_initial_heading"); // [[unused]] will be used in the future + p.smooth_reference = node->declare_parameter( + ns + ".smooth_reference_trajectory"); // [[unused]] will be used in the future + } + parameters_ = std::make_shared(p); +} + +void SamplingPlannerModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + [[maybe_unused]] const std::string ns = name_ + "."; + + { + std::string ns{"constraints.hard"}; + updateParam(parameters, ns + ".max_curvature", p->max_curvature); + updateParam(parameters, ns + ".min_curvature", p->min_curvature); + ns = std::string{"constraints.soft"}; + updateParam(parameters, ns + ".lateral_deviation_weight", p->lateral_deviation_weight); + updateParam(parameters, ns + ".length_weight", p->length_weight); + updateParam(parameters, ns + ".curvature_weight", p->curvature_weight); + updateParam>(parameters, ns + ".weights", p->weights); + } + { + std::string ns{"sampling"}; + updateParam(parameters, ns + ".enable_frenet", p->enable_frenet); + updateParam(parameters, ns + ".enable_bezier", p->enable_bezier); + updateParam(parameters, ns + ".resolution", p->resolution); + + updateParam( + parameters, ns + ".previous_path_reuse_points_nb", p->previous_path_reuse_points_nb); + + updateParam( + parameters, ns + ".nb_target_lateral_positions", p->nb_target_lateral_positions); + updateParam>(parameters, ns + ".target_lengths", p->target_lengths); + + updateParam>( + parameters, ns + ".target_lateral_positions", p->target_lateral_positions); + + ns += ".frenet"; + updateParam>( + parameters, ns + ".target_lateral_velocities", p->target_lateral_velocities); + + updateParam>( + parameters, ns + ".target_lateral_accelerations", p->target_lateral_accelerations); + } + { + std::string ns{"preprocessing"}; + updateParam(parameters, ns + ".force_zero_initial_deviation", p->force_zero_deviation); + updateParam(parameters, ns + ".force_zero_initial_heading", p->force_zero_heading); + updateParam(parameters, ns + ".smooth_reference_trajectory", p->smooth_reference); + } + + std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { + if (!observer.expired()) { + const auto sampling_planner_ptr = + std::dynamic_pointer_cast(observer.lock()); + if (sampling_planner_ptr) { + sampling_planner_ptr->updateModuleParams(p); + } + } + }); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::SamplingPlannerModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp new file mode 100644 index 0000000000000..73a726d8cdf7c --- /dev/null +++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -0,0 +1,978 @@ +// 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 "behavior_path_sampling_planner_module/sampling_planner_module.hpp" + +namespace behavior_path_planner +{ +using geometry_msgs::msg::Point; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; +using motion_utils::findNearestSegmentIndex; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::Point2d; + +namespace bg = boost::geometry; +namespace bgi = boost::geometry::index; + +SamplingPlannerModule::SamplingPlannerModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} +{ + internal_params_ = + std::shared_ptr(new SamplingPlannerInternalParameters{}); + updateModuleParams(parameters); + + // check if the path is empty + hard_constraints_.emplace_back( + []( + sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] const MultiPoint2d & footprint) -> bool { + return !path.points.empty() && !path.poses.empty(); + }); + + hard_constraints_.emplace_back( + []( + sampler_common::Path & path, const sampler_common::Constraints & constraints, + const MultiPoint2d & footprint) -> bool { + if (!footprint.empty()) { + path.constraint_results.drivable_area = + bg::within(footprint, constraints.drivable_polygons); + } + + for (const auto & f : footprint) { + const auto collision_index = constraints.rtree.qbegin(bgi::intersects(f)); + if (collision_index != constraints.rtree.qend()) { + path.constraint_results.collision = false; + break; + } + } + + return path.constraint_results.collision && path.constraint_results.drivable_area; + }); + + hard_constraints_.emplace_back( + []( + sampler_common::Path & path, const sampler_common::Constraints & constraints, + [[maybe_unused]] const MultiPoint2d & footprint) -> bool { + if (path.curvatures.empty()) { + path.constraint_results.curvature = false; + return false; + } + const bool curvatures_satisfied = + std::all_of(path.curvatures.begin(), path.curvatures.end(), [&](const auto & v) -> bool { + return (v > constraints.hard.min_curvature) && (v < constraints.hard.max_curvature); + }); + path.constraint_results.curvature = curvatures_satisfied; + return curvatures_satisfied; + }); + + // TODO(Daniel): Maybe add a soft cost for average distance to centerline? + // TODO(Daniel): Think of methods to prevent chattering + // TODO(Daniel): Add penalty for not ending up on the same line as ref path? + // TODO(Daniel): Length increasing curvature cost, increase curvature cost the longer the path is + // TODO(Daniel): in frenet path to path with laneID transform assign the laneid to the points from + // end to start -> that will prevent cases were some points on the output dont have a laneID + // TODO(Daniel): Set a goal that depends on the reference path and NOT the actual goal set by the + // user. + // Yaw difference soft constraint cost -> Considering implementation + // soft_constraints_.emplace_back( + // [&]( + // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & + // constraints, + // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { + // if (path.points.empty()) return 0.0; + // const auto & goal_pose_yaw = + // tier4_autoware_utils::getRPY(input_data.goal_pose.orientation).z; const auto & + // last_point_yaw = path.yaws.back(); const double angle_difference = std::abs(last_point_yaw + // - goal_pose_yaw); return angle_difference / (3.141519 / 4.0); + // }); + + // Remaining path length + soft_constraints_.emplace_back( + [&]( + sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { + if (path.points.empty()) return 0.0; + if (path.poses.empty()) return 0.0; + + const auto & ego_pose = input_data.ego_pose; + + const double max_target_length = *std::max_element( + internal_params_->sampling.target_lengths.begin(), + internal_params_->sampling.target_lengths.end()); + + const auto closest_path_pose_itr = std::min_element( + path.poses.begin(), path.poses.end(), [&ego_pose](const auto & p1, const auto & p2) { + const auto dist1 = + std::hypot(p1.position.x - ego_pose.position.x, p1.position.y - ego_pose.position.y); + const auto dist2 = + std::hypot(p2.position.x - ego_pose.position.x, p2.position.y - ego_pose.position.y); + return dist1 < dist2; + }); + + int closest_path_pose_index = static_cast(closest_path_pose_itr - path.poses.begin()); + + const double remaining_path_length = + path.lengths.back() - path.lengths.at(closest_path_pose_index); + + constexpr double epsilon = 1E-5; + if (remaining_path_length < epsilon) return max_target_length; + return max_target_length / remaining_path_length; + }); + + // Distance to centerline + soft_constraints_.emplace_back( + [&]( + [[maybe_unused]] sampler_common::Path & path, + [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { + if (path.poses.empty()) return 0.0; + const auto & last_pose = path.poses.back(); + const auto path_point_arc = + lanelet::utils::getArcCoordinates(input_data.closest_lanelets_to_goal, last_pose); + const double lateral_distance_to_center_lane = std::abs(path_point_arc.distance); + const double max_target_lateral_positions = *std::max_element( + internal_params_->sampling.target_lateral_positions.begin(), + internal_params_->sampling.target_lateral_positions.end()); + return lateral_distance_to_center_lane / max_target_lateral_positions; + }); + + // // Curvature cost + soft_constraints_.emplace_back( + []( + sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { + if (path.curvatures.empty()) return std::numeric_limits::max(); + + const double max_path_curvature = *std::max_element( + path.curvatures.begin(), path.curvatures.end(), + [](const auto & a, const auto & b) { return std::abs(a) < std::abs(b); }); + return std::abs(max_path_curvature) / constraints.hard.max_curvature; + }); +} + +bool SamplingPlannerModule::isExecutionRequested() const +{ + const auto ego_pose = planner_data_->self_odometry->pose.pose; + lanelet::ConstLanelet current_lane; + + if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "failed to find closest lanelet within route!!!"); + return false; + } + + if (getPreviousModuleOutput().reference_path.points.empty()) { + return false; + } + + if (!motion_utils::isDrivingForward(getPreviousModuleOutput().reference_path.points)) { + RCLCPP_WARN(getLogger(), "Backward path is NOT supported. Just converting path to trajectory"); + return false; + } + + return !isReferencePathSafe(); +} + +bool SamplingPlannerModule::isReferencePathSafe() const +{ + // TODO(Daniel): Don't use reference path, use a straight path forward. + std::vector drivable_lanes{}; + const auto & prev_module_reference_path = + std::make_shared(getPreviousModuleOutput().reference_path); + + const auto & p = planner_data_->parameters; + const auto ego_pose = planner_data_->self_odometry->pose.pose; + lanelet::ConstLanelet current_lane; + + if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "failed to find closest lanelet within route!!!"); + return {}; + } + const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence( + current_lane, ego_pose, p.backward_path_length, p.forward_path_length); + // expand drivable lanes + std::for_each( + current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) { + drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_)); + }); + + lanelet::ConstLanelets current_lanes; + + for (auto & d : drivable_lanes) { + current_lanes.push_back(d.right_lane); + current_lanes.push_back(d.left_lane); + current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end()); + } + + { + const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path; + const auto left_bound = (utils::calcBound( + path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true)); + const auto right_bound = (utils::calcBound( + path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false)); + + const auto sampling_planner_data = + createPlannerData(planner_data_->prev_output_path, left_bound, right_bound); + + prepareConstraints( + internal_params_->constraints, planner_data_->dynamic_object, + sampling_planner_data.left_bound, sampling_planner_data.right_bound); + } + + std::vector hard_constraints_results; + auto transform_to_sampling_path = [](const PlanResult plan) { + sampler_common::Path path; + for (size_t i = 0; i < plan->points.size(); ++i) { + const auto x = plan->points[i].point.pose.position.x; + const auto y = plan->points[i].point.pose.position.y; + const auto quat = plan->points[i].point.pose.orientation; + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + path.points.emplace_back(Point2d{x, y}); + path.poses.emplace_back(plan->points[i].point.pose); + path.yaws.emplace_back(rpy.z); + } + return path; + }; + sampler_common::Path reference_path = transform_to_sampling_path(prev_module_reference_path); + const auto footprint = sampler_common::constraints::buildFootprintPoints( + reference_path, internal_params_->constraints); + + behavior_path_planner::HardConstraintsFunctionVector hard_constraints_reference_path; + hard_constraints_reference_path.emplace_back( + []( + sampler_common::Path & path, const sampler_common::Constraints & constraints, + const MultiPoint2d & footprint) -> bool { + path.constraint_results.collision = + !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons); + return path.constraint_results.collision; + }); + evaluateHardConstraints( + reference_path, internal_params_->constraints, footprint, hard_constraints_reference_path); + return reference_path.constraints_satisfied; +} + +bool SamplingPlannerModule::isExecutionReady() const +{ + return true; +} + +SamplingPlannerData SamplingPlannerModule::createPlannerData( + const PlanResult & path, const std::vector & left_bound, + const std::vector & right_bound) const +{ + SamplingPlannerData data; + auto points = path->points; + data.left_bound = left_bound; + data.right_bound = right_bound; + data.ego_pose = planner_data_->self_odometry->pose.pose; + data.ego_vel = planner_data_->self_odometry->twist.twist.linear.x; + return data; +} + +PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( + const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const double path_z) +{ + auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion { + tf2::Quaternion quaternion_tf2; + quaternion_tf2.setRPY(roll, pitch, yaw); + return quaternion_tf2; + }; + + PathWithLaneId path; + const auto header = planner_data_->route_handler->getRouteHeader(); + const auto reference_path_ptr = + std::make_shared(getPreviousModuleOutput().reference_path); + + for (size_t i = 0; i < frenet_path.points.size(); ++i) { + const auto & frenet_path_point_position = frenet_path.points.at(i); + const auto & frenet_path_point_yaw = frenet_path.yaws.at(i); + PathPointWithLaneId point{}; + point.point.pose.position.x = frenet_path_point_position.x(); + point.point.pose.position.y = frenet_path_point_position.y(); + point.point.pose.position.z = path_z; + + auto yaw_as_quaternion = quaternion_from_rpy(0.0, 0.0, frenet_path_point_yaw); + point.point.pose.orientation.w = yaw_as_quaternion.getW(); + point.point.pose.orientation.x = yaw_as_quaternion.getX(); + point.point.pose.orientation.y = yaw_as_quaternion.getY(); + point.point.pose.orientation.z = yaw_as_quaternion.getZ(); + + // put the lane that contain waypoints in lane_ids. + bool is_in_lanes = false; + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + is_in_lanes = true; + } + } + // If none of them corresponds, assign the previous lane_ids. + if (!is_in_lanes && i > 0) { + point.lane_ids = path.points.at(i - 1).lane_ids; + } + if (reference_path_ptr) { + const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + reference_path_ptr->points, point.point.pose); + const auto & closest_point = reference_path_ptr->points[idx]; + point.point.longitudinal_velocity_mps = closest_point.point.longitudinal_velocity_mps; + point.point.lateral_velocity_mps = closest_point.point.lateral_velocity_mps; + } + path.points.push_back(point); + } + return path; +} + +void SamplingPlannerModule::prepareConstraints( + sampler_common::Constraints & constraints, + const PredictedObjects::ConstSharedPtr & predicted_objects, + const std::vector & left_bound, + const std::vector & right_bound) const +{ + constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.rtree.clear(); + size_t i = 0; + for (const auto & o : predicted_objects->objects) { + if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) { + const auto polygon = tier4_autoware_utils::toPolygon2d(o); + constraints.obstacle_polygons.push_back(polygon); + const auto box = boost::geometry::return_envelope(polygon); + constraints.rtree.insert(std::make_pair(box, i)); + } + i++; + } + + constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented + + // TODO(Maxime): directly use lines instead of polygon + + sampler_common::Polygon2d drivable_area_polygon; + for (const auto & p : right_bound) { + drivable_area_polygon.outer().emplace_back(p.x, p.y); + } + for (auto it = left_bound.rbegin(); it != left_bound.rend(); ++it) + drivable_area_polygon.outer().emplace_back(it->x, it->y); + + if (drivable_area_polygon.outer().size() < 1) { + return; + } + drivable_area_polygon.outer().push_back(drivable_area_polygon.outer().front()); + + constraints.drivable_polygons = {drivable_area_polygon}; +} + +BehaviorModuleOutput SamplingPlannerModule::plan() +{ + const auto reference_path_ptr = + std::make_shared(getPreviousModuleOutput().reference_path); + if (reference_path_ptr->points.empty()) { + return {}; + } + auto reference_spline = [&]() -> sampler_common::transform::Spline2D { + std::vector x; + std::vector y; + x.reserve(reference_path_ptr->points.size()); + y.reserve(reference_path_ptr->points.size()); + for (const auto & point : reference_path_ptr->points) { + x.push_back(point.point.pose.position.x); + y.push_back(point.point.pose.position.y); + } + return {x, y}; + }(); + + frenet_planner::FrenetState frenet_initial_state; + frenet_planner::SamplingParameters sampling_parameters; + + const auto & pose = planner_data_->self_odometry->pose.pose; + sampler_common::State initial_state = + behavior_path_planner::getInitialState(pose, reference_spline); + sampling_parameters = + prepareSamplingParameters(initial_state, reference_spline, *internal_params_); + + auto set_frenet_state = []( + const sampler_common::State & initial_state, + const sampler_common::transform::Spline2D & reference_spline, + frenet_planner::FrenetState & frenet_initial_state) + + { + frenet_initial_state.position = initial_state.frenet; + const auto frenet_yaw = initial_state.heading - reference_spline.yaw(initial_state.frenet.s); + const auto path_curvature = reference_spline.curvature(initial_state.frenet.s); + constexpr auto delta_s = 0.001; + frenet_initial_state.lateral_velocity = + (1 - path_curvature * initial_state.frenet.d) * std::tan(frenet_yaw); + const auto path_curvature_deriv = + (reference_spline.curvature(initial_state.frenet.s + delta_s) - path_curvature) / delta_s; + const auto cos_yaw = std::cos(frenet_yaw); + if (cos_yaw == 0.0) { + frenet_initial_state.lateral_acceleration = 0.0; + } else { + frenet_initial_state.lateral_acceleration = + -(path_curvature_deriv * initial_state.frenet.d + + path_curvature * frenet_initial_state.lateral_velocity) * + std::tan(frenet_yaw) + + ((1 - path_curvature * initial_state.frenet.d) / (cos_yaw * cos_yaw)) * + (initial_state.curvature * ((1 - path_curvature * initial_state.frenet.d) / cos_yaw) - + path_curvature); + } + }; + + set_frenet_state(initial_state, reference_spline, frenet_initial_state); + + const auto prev_module_path = std::make_shared(getPreviousModuleOutput().path); + + const auto & p = planner_data_->parameters; + const auto ego_pose = planner_data_->self_odometry->pose.pose; + lanelet::ConstLanelet current_lane; + + if (!planner_data_->route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "failed to find closest lanelet within route!!!"); + return getPreviousModuleOutput(); + } + + std::vector drivable_lanes{}; + const auto current_lane_sequence = planner_data_->route_handler->getLaneletSequence( + current_lane, ego_pose, p.backward_path_length, p.forward_path_length); + // expand drivable lanes + + std::for_each( + current_lane_sequence.begin(), current_lane_sequence.end(), [&](const auto & lanelet) { + drivable_lanes.push_back(generateExpandDrivableLanes(lanelet, planner_data_)); + }); + + lanelet::ConstLanelets current_lanes; + + for (auto & d : drivable_lanes) { + current_lanes.push_back(d.right_lane); + current_lanes.push_back(d.left_lane); + current_lanes.insert(current_lanes.end(), d.middle_lanes.begin(), d.middle_lanes.end()); + } + + auto get_goal_pose = [&]() { + auto goal_pose = planner_data_->route_handler->getGoalPose(); + if (!std::any_of(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) { + return lanelet::utils::isInLanelet(goal_pose, lane); + })) { + for (auto it = reference_path_ptr->points.rbegin(); it < reference_path_ptr->points.rend(); + it++) { + if (std::any_of(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) { + return lanelet::utils::isInLanelet(it->point.pose, lane); + })) { + goal_pose = it->point.pose; + break; + } + } + } + return goal_pose; + }; + + const bool prev_path_is_valid = prev_sampling_path_ && prev_sampling_path_->points.size() > 0; + const int path_divisions = internal_params_->sampling.previous_path_reuse_points_nb; + const bool is_extend_previous_path = path_divisions > 0; + + std::vector frenet_paths; + // Extend prev path + if (prev_path_is_valid && is_extend_previous_path) { + frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); + frenet_paths.push_back(prev_path_frenet); + + auto get_subset = [](const frenet_planner::Path & path, size_t offset) -> frenet_planner::Path { + frenet_planner::Path s; + s.points = {path.points.begin(), path.points.begin() + offset}; + s.curvatures = {path.curvatures.begin(), path.curvatures.begin() + offset}; + s.yaws = {path.yaws.begin(), path.yaws.begin() + offset}; + s.lengths = {path.lengths.begin(), path.lengths.begin() + offset}; + s.poses = {path.poses.begin(), path.poses.begin() + offset}; + return s; + }; + + sampler_common::State current_state; + current_state.pose = {ego_pose.position.x, ego_pose.position.y}; + + const auto closest_iter = std::min_element( + prev_path_frenet.points.begin(), prev_path_frenet.points.end() - 1, + [&](const auto & p1, const auto & p2) { + return boost::geometry::distance(p1, current_state.pose) <= + boost::geometry::distance(p2, current_state.pose); + }); + + const auto current_idx = std::distance(prev_path_frenet.points.begin(), closest_iter); + const double current_length = prev_path_frenet.lengths.at(current_idx); + const double remaining_path_length = prev_path_frenet.lengths.back() - current_length; + const double length_step = remaining_path_length / path_divisions; + for (double reuse_length = 0.0; reuse_length <= remaining_path_length; + reuse_length += length_step) { + size_t reuse_idx; + for (reuse_idx = current_idx + 1; reuse_idx + 2 < prev_path_frenet.lengths.size(); + ++reuse_idx) { + if (prev_path_frenet.lengths[reuse_idx] - current_length >= reuse_length) break; + } + + const auto reused_path = get_subset(prev_path_frenet, reuse_idx); + + geometry_msgs::msg::Pose future_pose = reused_path.poses.back(); + sampler_common::State future_state = + behavior_path_planner::getInitialState(future_pose, reference_spline); + frenet_planner::FrenetState frenet_reuse_state; + + set_frenet_state(future_state, reference_spline, frenet_reuse_state); + frenet_planner::SamplingParameters extension_sampling_parameters = + prepareSamplingParameters(future_state, reference_spline, *internal_params_); + auto extension_frenet_paths = frenet_planner::generatePaths( + reference_spline, frenet_reuse_state, extension_sampling_parameters); + for (auto & p : extension_frenet_paths) { + if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p)); + } + } + } else { + frenet_paths = + frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters); + } + + const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path; + const auto left_bound = (utils::calcBound( + path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true)); + const auto right_bound = (utils::calcBound( + path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false)); + + const auto sampling_planner_data = + createPlannerData(planner_data_->prev_output_path, left_bound, right_bound); + + prepareConstraints( + internal_params_->constraints, planner_data_->dynamic_object, sampling_planner_data.left_bound, + sampling_planner_data.right_bound); + + SoftConstraintsInputs soft_constraints_input; + const auto & goal_pose = get_goal_pose(); + soft_constraints_input.goal_pose = soft_constraints_input.ego_pose = + planner_data_->self_odometry->pose.pose; + + soft_constraints_input.current_lanes = current_lanes; + soft_constraints_input.reference_path = reference_path_ptr; + soft_constraints_input.prev_module_path = prev_module_path; + + soft_constraints_input.ego_arc = lanelet::utils::getArcCoordinates(current_lanes, ego_pose); + soft_constraints_input.goal_arc = lanelet::utils::getArcCoordinates(current_lanes, goal_pose); + lanelet::ConstLanelet closest_lanelet_to_goal; + lanelet::utils::query::getClosestLanelet(current_lanes, goal_pose, &closest_lanelet_to_goal); + soft_constraints_input.closest_lanelets_to_goal = {closest_lanelet_to_goal}; + + debug_data_.footprints.clear(); + std::vector> hard_constraints_results_full; + std::vector> soft_constraints_results_full; + for (auto & path : frenet_paths) { + const auto footprint = + sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints); + std::vector hard_constraints_results = + evaluateHardConstraints(path, internal_params_->constraints, footprint, hard_constraints_); + path.constraint_results.curvature = true; + debug_data_.footprints.push_back(footprint); + std::vector soft_constraints_results = evaluateSoftConstraints( + path, internal_params_->constraints, soft_constraints_, soft_constraints_input); + soft_constraints_results_full.push_back(soft_constraints_results); + } + + std::vector candidate_paths; + const auto move_to_paths = [&candidate_paths](auto & paths_to_move) { + candidate_paths.insert( + candidate_paths.end(), std::make_move_iterator(paths_to_move.begin()), + std::make_move_iterator(paths_to_move.end())); + }; + + move_to_paths(frenet_paths); + debug_data_.previous_sampled_candidates_nb = debug_data_.sampled_candidates.size(); + debug_data_.sampled_candidates = candidate_paths; + debug_data_.obstacles = internal_params_->constraints.obstacle_polygons; + updateDebugMarkers(); + + const double max_length = *std::max_element( + internal_params_->sampling.target_lengths.begin(), + internal_params_->sampling.target_lengths.end()); + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, max_length, max_length, false); // Do these max lengths make sense? + + const auto best_path_idx = [](const auto & paths) { + auto min_cost = std::numeric_limits::max(); + size_t best_path_idx = 0; + for (auto i = 0LU; i < paths.size(); ++i) { + if (paths[i].constraints_satisfied && paths[i].cost < min_cost) { + best_path_idx = i; + min_cost = paths[i].cost; + } + } + return min_cost < std::numeric_limits::max() ? std::optional(best_path_idx) + : std::nullopt; + }; + + const auto selected_path_idx = best_path_idx(frenet_paths); + + if (!selected_path_idx) { + BehaviorModuleOutput out; + PathWithLaneId out_path = (prev_sampling_path_) + ? convertFrenetPathToPathWithLaneID( + prev_sampling_path_.value(), current_lanes, + planner_data_->route_handler->getGoalPose().position.z) + : PathWithLaneId{}; + + out.path = (prev_sampling_path_) ? out_path : getPreviousModuleOutput().path; + out.reference_path = getPreviousModuleOutput().reference_path; + out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; + extendOutputDrivableArea(out, drivable_lanes); + return out; + } + + const auto best_path = frenet_paths[*selected_path_idx]; + + std::cerr << "Soft constraints results of best: "; + for (const auto result : soft_constraints_results_full[*selected_path_idx]) + std::cerr << result << ","; + std::cerr << "\n"; + + std::cerr << "Poses " << best_path.poses.size() << "\n"; + std::cerr << "Length of best " << best_path.lengths.back() << "\n"; + + const auto out_path = convertFrenetPathToPathWithLaneID( + best_path, current_lanes, planner_data_->route_handler->getGoalPose().position.z); + prev_sampling_path_ = best_path; + + std::cerr << "road_lanes size " << road_lanes.size() << "\n"; + std::cerr << "First lane ID size " << out_path.points.at(0).lane_ids.size() << "\n"; + BehaviorModuleOutput out; + out.path = out_path; + out.reference_path = *reference_path_ptr; + out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; + extendOutputDrivableArea(out, drivable_lanes); + return out; +} + +void SamplingPlannerModule::updateDebugMarkers() +{ + debug_marker_.markers.clear(); + info_marker_.markers.clear(); + + const auto header = planner_data_->route_handler->getRouteHeader(); + visualization_msgs::msg::Marker m; + m.header.frame_id = "map"; + m.header.stamp = header.stamp; + m.action = m.ADD; + m.id = 0UL; + m.type = m.LINE_STRIP; + m.color.a = 1.0; + m.scale.x = 0.02; + m.ns = "candidates"; + for (const auto & c : debug_data_.sampled_candidates) { + m.points.clear(); + for (const auto & p : c.points) + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + if (c.constraint_results.isValid()) { + m.color.g = 1.0; + m.color.r = 0.0; + } else { + m.color.r = 1.0; + m.color.g = 0.0; + } + debug_marker_.markers.push_back(m); + info_marker_.markers.push_back(m); + ++m.id; + } + m.ns = "footprint"; + m.id = 0UL; + m.type = m.POINTS; + m.points.clear(); + m.color.a = 1.0; + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 1.0; + m.scale.y = 0.2; + if (!debug_data_.footprints.empty()) { + m.action = m.ADD; + for (const auto & p : debug_data_.footprints[0]) { + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + } + + } else { + m.action = m.DELETE; + } + debug_marker_.markers.push_back(m); + info_marker_.markers.push_back(m); + ++m.id; + m.type = m.LINE_STRIP; + m.ns = "obstacles"; + m.id = 0UL; + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 0.0; + for (const auto & obs : debug_data_.obstacles) { + m.points.clear(); + for (const auto & p : obs.outer()) + m.points.push_back(geometry_msgs::msg::Point().set__x(p.x()).set__y(p.y())); + debug_marker_.markers.push_back(m); + info_marker_.markers.push_back(m); + ++m.id; + } + m.action = m.DELETE; + m.ns = "candidates"; + for (m.id = debug_data_.sampled_candidates.size(); + static_cast(m.id) < debug_data_.previous_sampled_candidates_nb; ++m.id) { + debug_marker_.markers.push_back(m); + info_marker_.markers.push_back(m); + } +} + +void SamplingPlannerModule::extendOutputDrivableArea( + BehaviorModuleOutput & output, std::vector & drivable_lanes) +{ + // // for new architecture + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); +} + +CandidateOutput SamplingPlannerModule::planCandidate() const +{ + return {}; +} + +void SamplingPlannerModule::updateData() +{ +} + +// utils + +template +void pushUniqueVector(T & base_vector, const T & additional_vector) +{ + base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); +} + +bool SamplingPlannerModule::isEndPointsConnected( + const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) const +{ + const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint(); + const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint(); + + constexpr double epsilon = 1e-5; + return (right_back_point_2d - left_back_point_2d).norm() < epsilon; +} + +DrivableLanes SamplingPlannerModule::generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, + const std::shared_ptr & planner_data) const +{ + const auto & route_handler = planner_data->route_handler; + + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = lanelet; + current_drivable_lanes.right_lane = lanelet; + + // 1. get left/right side lanes + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_left_lanelets = + route_handler->getAllLeftSharedLinestringLanelets(target_lane, true, true); + if (!all_left_lanelets.empty()) { + current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1)); + } + }; + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_right_lanelets = + route_handler->getAllRightSharedLinestringLanelets(target_lane, true, true); + if (!all_right_lanelets.empty()) { + current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1)); + } + }; + + update_left_lanelets(lanelet); + update_right_lanelets(lanelet); + + // 2.1 when there are multiple lanes whose previous lanelet is the same + const auto get_next_lanes_from_same_previous_lane = + [&route_handler](const lanelet::ConstLanelet & lane) { + // get previous lane, and return false if previous lane does not exist + lanelet::ConstLanelets prev_lanes; + if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { + return lanelet::ConstLanelets{}; + } + + lanelet::ConstLanelets next_lanes; + for (const auto & prev_lane : prev_lanes) { + const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); + pushUniqueVector(next_lanes, next_lanes_from_prev); + } + return next_lanes; + }; + + const auto next_lanes_for_right = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane); + const auto next_lanes_for_left = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); + + // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line + // of the original lane + const auto update_drivable_lanes = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + for (const auto & next_lane : next_lanes) { + const auto & edge_lane = + is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane; + if (next_lane.id() == edge_lane.id()) { + continue; + } + + const auto & left_lane = is_left ? next_lane : edge_lane; + const auto & right_lane = is_left ? edge_lane : next_lane; + if (!isEndPointsConnected(left_lane, right_lane)) { + continue; + } + + if (is_left) { + current_drivable_lanes.left_lane = next_lane; + } else { + current_drivable_lanes.right_lane = next_lane; + } + + const auto & middle_lanes = current_drivable_lanes.middle_lanes; + const auto has_same_lane = std::any_of( + middle_lanes.begin(), middle_lanes.end(), + [&edge_lane](const auto & lane) { return lane.id() == edge_lane.id(); }); + + if (!has_same_lane) { + if (is_left) { + if (current_drivable_lanes.right_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } else { + if (current_drivable_lanes.left_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } + } + + return true; + } + return false; + }; + + const auto expand_drivable_area_recursively = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + // NOTE: set max search num to avoid infinity loop for drivable area expansion + constexpr size_t max_recursive_search_num = 3; + for (size_t i = 0; i < max_recursive_search_num; ++i) { + const bool is_update_kept = update_drivable_lanes(next_lanes, is_left); + if (!is_update_kept) { + break; + } + if (i == max_recursive_search_num - 1) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "Drivable area expansion reaches max iteration."); + } + } + }; + expand_drivable_area_recursively(next_lanes_for_right, false); + expand_drivable_area_recursively(next_lanes_for_left, true); + + // 3. update again for new left/right lanes + update_left_lanelets(current_drivable_lanes.left_lane); + update_right_lanelets(current_drivable_lanes.right_lane); + + // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes. + if ( + current_drivable_lanes.left_lane.id() != lanelet.id() && + current_drivable_lanes.right_lane.id() != lanelet.id()) { + current_drivable_lanes.middle_lanes.push_back(lanelet); + } + + return current_drivable_lanes; +} + +frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( + const sampler_common::State & initial_state, + const sampler_common::transform::Spline2D & path_spline, + const SamplingPlannerInternalParameters & params_) +{ + // calculate target lateral positions + std::vector target_lateral_positions; + if (params_.sampling.nb_target_lateral_positions > 1) { + target_lateral_positions = {0.0, initial_state.frenet.d}; + double min_d = 0.0; + double max_d = 0.0; + double min_d_s = std::numeric_limits::max(); + double max_d_s = std::numeric_limits::max(); + for (const auto & drivable_poly : params_.constraints.drivable_polygons) { + for (const auto & p : drivable_poly.outer()) { + const auto frenet_coordinates = path_spline.frenet(p); + const auto d_s = std::abs(frenet_coordinates.s - initial_state.frenet.s); + if (d_s < min_d_s && frenet_coordinates.d < 0.0) { + min_d_s = d_s; + min_d = frenet_coordinates.d; + } + if (d_s < max_d_s && frenet_coordinates.d > 0.0) { + max_d_s = d_s; + max_d = frenet_coordinates.d; + } + } + } + min_d += params_.constraints.ego_width / 2.0; + max_d -= params_.constraints.ego_width / 2.0; + if (min_d < max_d) { + for (auto r = 0.0; r <= 1.0; r += 1.0 / (params_.sampling.nb_target_lateral_positions - 1)) + target_lateral_positions.push_back(interpolation::lerp(min_d, max_d, r)); + } + } else { + target_lateral_positions = params_.sampling.target_lateral_positions; + } + frenet_planner::SamplingParameters sampling_parameters; + sampling_parameters.resolution = params_.sampling.resolution; + const auto max_s = path_spline.lastS(); + frenet_planner::SamplingParameter p; + for (const auto target_length : params_.sampling.target_lengths) { + p.target_state.position.s = + std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length)); + for (const auto target_lateral_position : target_lateral_positions) { + p.target_state.position.d = target_lateral_position; + for (const auto target_lat_vel : params_.sampling.frenet.target_lateral_velocities) { + p.target_state.lateral_velocity = target_lat_vel; + for (const auto target_lat_acc : params_.sampling.frenet.target_lateral_accelerations) { + p.target_state.lateral_acceleration = target_lat_acc; + sampling_parameters.parameters.push_back(p); + } + } + } + if (p.target_state.position.s == max_s) break; + } + return sampling_parameters; +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 7d72afebfb359..1748f57a61bec 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -75,8 +75,6 @@ class SideShiftModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } - void initVariables(); // non-const methods diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index b26864eb450d5..69be5c3723baf 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -2,24 +2,278 @@ ## Purpose / Role -The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and stopping in response to dynamic obstacles when a collision is detected. +This module generates and plans a path for safely merging from the shoulder lane or side of road lane into the center of the road lane. -Use cases include: +Specifically, it includes the following features: -- pull out from the side of the road lane to centerline. +- Plan the path to automatically start from the shoulder lane or side of road lane to center of road lane. +- When parked vehicles are present on the shoulder lane, the module generates a path that allows for starting with a gap of a specified margin. +- If a collision with other traffic participants is detected while traveling on the generated path, it will stop as much as possible.
- ![case1](images/start_from_road_side.drawio.svg){width=1000} -
pull out from side of the road lane
+ ![start_planner_module](images/start_planner_example.png){width=1100}
-- pull out from the shoulder lane to the road lane centerline. +## Use Cases + +Give an typical example of how path generation is executed. Showing example of path generation starts from shoulder lane, but also from side of road lane can be generated. + +
+ ![shift_pull_out_from_road_lane](images/shift_pull_out_path_from_road_lane.drawio.svg){width=1100} +
+ +### **Use Case 1: Shift pull out** + +In the shoulder lane, when there are no parked vehicles ahead and the shoulder lane is sufficiently long, a forward shift pull out path (a clothoid curve with consistent jerk) is generated. + +
+ ![shift_pull_out](images/shift_pull_out_path.drawio.svg){width=1100} +
+ +### **Use Case 2: Geometric pull out** + +In the shoulder lane, when there are objects in front and the lane is not sufficiently long behind, a geometric pull out path is generated. + +
+ ![geometric_pull_out](images/geometric_pull_out_path.drawio.svg){width=1100} +
+ +### **Use Case 3: Backward and shift pull out** + +In the shoulder lane, when there are parked vehicles ahead and the lane is sufficiently long behind, a path that involves reversing before generating a forward shift pull out path is created. + +
+ ![shift_pull_out_with_back](images/shift_pull_out_path_with_back.drawio.svg){width=1100} +
+ +### **Use Case 4: Backward and geometric pull out** + +In the shoulder lane, when there is an object ahead and not enough space to reverse sufficiently, a path that involves reversing before making an geometric pull out is generated. + +
+ ![geometric_pull_out_with_back](images/geometric_pull_out_path_with_back.drawio.svg){width=1100} +
+ +### **Use Case 5: Freespace pull out** + +If the map is annotated with the information that a free space path can be generated in situations where both shift and geometric pull out paths are impossible to create, a path based on the free space algorithm will be generated. + +
+ ![freespace_path](images/freespace_pull_out.png){width=1100} +
+ +**As a note, the patterns for generating these paths are based on default parameters, but as will be explained in the following sections, it is possible to control aspects such as making paths that involve reversing more likely to be generated, or making geometric paths more likely to be generated, by changing the path generation policy or adjusting the margin around static objects.** + +## Limitations + +Here are some notable limitations: + +- If parked vehicles exist in front of, behind, or on both sides of the shoulder, and it's not possible to maintain a specified distance from them, a stopping path will be generated, leading to a potential deadlock. +- In the default settings of the behavior_path_planner, an avoidance module operates in a later stage and attempts to avoid objects. However, it is not guaranteed that the start_planner module will output a path that can successfully navigate around obstacles. This means that if an unavoidable path is generated, it could result in a deadlock. +- The performance of safety check relies on the accuracy of the predicted paths generated by the map_based_prediction node. It's important to note that, currently, predicted paths that consider acceleration are not generated, and paths for low-speed objects may not be accurately produced, which requires caution. +- Given the current specifications of the rule-based algorithms, there is a trade-off between the safety of a path and its naturalness to humans. While it's possible to adjust parameters to manage this trade-off, improvements are necessary to better reconcile these aspects. + +## Start/End Conditions + +### **Start Conditions** + +The `StartPlannerModule` is designed to initiate its execution based on specific criteria evaluated by the `isExecutionRequested` function. The module will **not** start under the following conditions: + +1. **Start pose on the middle of the road**: The module will not initiate if the start pose of the vehicle is determined to be in the middle of the road. This ensures the planner starts from a roadside position. + +2. **Vehicle is far from start position**: If the vehicle is far from the start position, the module will not execute. This prevents redundant execution when the new goal is given. + +3. **Vehicle reached goal**: The module will not start if the vehicle has already reached its goal position, avoiding unnecessary execution when the destination is attained. + +4. **Vehicle in motion**: If the vehicle is still moving, the module will defer starting. This ensures that planning occurs from a stable, stationary state for safety. + +5. **Goal behind in same route segment**: The module will not initiate if the goal position is behind the ego vehicle within the same route segment. This condition is checked to avoid complications with planning routes that require the vehicle to move backward on its current path, which is currently not supported. + +### **End Conditions** + +The `StartPlannerModule` terminates if the pull out / freespace maneuver has been completed. The `canTransitSuccessState` function assesses these conditions to decide if the module should terminate its execution. + +```plantuml +@startuml +start +:Start hasFinishedPullOut(); + +if (status_.driving_forward && status_.found_pull_out_path) then (yes) + + if (status_.planner_type == FREESPACE) then (yes) + :Calculate distance\nto pull_out_path.end_pose; + if (distance < th_arrived_distance) then (yes) + :return true;\n(Terminate module); + else (no) + :return false;\n(Continue running); + endif + else (no) + :Calculate arclength for\ncurrent_pose and pull_out_path.end_pose; + if (arclength_current - arclength_pull_out_end > offset) then (yes) + :return true;\n(Terminate module); + else (no) + :return false;\n(Continue running); + endif + endif + +else (no) + :return false;\n(Continue running); +endif + +stop +@enduml +``` + +## Concept of safety assurance + +The approach to collision safety is divided into two main components: generating paths that consider static information, and detecting collisions with dynamic obstacles to ensure the safety of the generated paths. + +### 1. Generating path with static information + +- **Path deviation checks**: This ensures that the path remains within the designated lanelets. By default, this feature is active, but it can be deactivated if necessary. + +- **Static obstacle clearance from the path**: This involves verifying that a sufficient margin around static obstacles is maintained. The process includes creating a vehicle-sized footprint from the current position to the pull-out endpoint, which can be adjusted via parameters. The distance to static obstacle polygons is then calculated. If this distance is below a specified threshold, the path is deemed unsafe. Threshold levels (e.g., [2.0, 1.0, 0.5, 0.1]) can be configured, and the system searches for paths that meet the highest possible threshold based on a set search priority explained in following section, ensuring the selection of the safe path based on the policy. If no path meets the minimum threshold, it's determined that no safe path is available. + +- **Clearance from stationary objects**: Maintaining an adequate distance from stationary objects positioned in front of and behind the vehicle is imperative for safety. Despite the path and stationary objects having a confirmed margin, the path is deemed unsafe if the distance from the shift start position to a front stationary object falls below `collision_check_margin_from_front_object` meters, or if the distance to a rear stationary object is shorter than `back_objects_collision_check_margin` meters. + + - Why is a margin from the front object necessary? + Consider a scenario in a "geometric pull out path" where the clearance from the path to a static obstacle is minimal, and there is a stopped vehicle ahead. In this case, although the path may meet safety standards and thus be generated, a concurrently operating avoidance module might deem it impossible to avoid the obstacle, potentially leading to vehicle deadlock. To ensure there is enough distance for avoidance maneuvers, the distance to the front obstacle is assessed. Increasing this parameter can prevent immobilization within the avoidance module but may also lead to the frequent generation of backward paths or geometric pull out path, resulting in paths that may seem unnatural to humans. + + - Why is a margin from the rear object necessary? + For objects ahead, another behavior module can intervene, allowing the path to overwrite itself through an avoidance plan, even if the clearance from the path to a static obstacle is minimal, thus maintaining a safe distance from static obstacles. However, for objects behind the vehicle, it is impossible for other behavior modules other than the start_planner to alter the path to secure a margin, potentially leading to a deadlock by an action module like "obstacle_cruise_planner" and subsequent immobilization. Therefore, a margin is set for stationary objects at the rear. + +Here's the expression of the steps start pose searching steps, considering the `collision_check_margins` is set at [2.0, 1.0, 0.5, 0.1] as example. The process is as follows: + +1. **Generating start pose candidates** + + - Set the current position of the vehicle as the base point. + - Determine the area of consideration behind the vehicle up to the `max_back_distance`. + - Generate candidate points for the start pose in the backward direction at intervals defined by `backward_search_resolution`. + - Include the current position as one of the start pose candidates. + + ![start pose candidate](images/start_pose_candidate.drawio.svg){width=1100} + +2. **Starting search at maximum margin** + + - Begin the search with the largest threshold (e.g., 2.0 meters). + - Evaluate each start pose candidate to see if it maintains a margin of more than 2.0 meters. + - Simultaneously, verify that the path generated from that start pose meets other necessary criteria (e.g., path deviation check). + - Following the search priority described later, evaluate each in turn and adopt the start pose if it meets the conditions. + +3. **Repeating search according to threshold levels** + + - If no start pose meeting the conditions is found, lower the threshold to the next level (e.g., 1.0 meter) and repeat the search. + +4. **Continuing the search** + + - Continue the search until a start pose that meets the conditions is found, or the threshold level reaches the minimum value (e.g., 0.1 meter). + - The aim of this process is to find a start pose that not only secures as large a margin as possible but also satisfies the conditions required for the path. + +5. **Generating a stop path** + - If no start pose satisfies the conditions at any threshold level, generate a stop path to ensure safety. + +#### **search priority** + +If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. + +During this backward search, different policies can be applied based on `search_priority` parameters: + +Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. +Opting for `short_back_distance` aims to find a location with the least possible backward movement. + +![priority_order](./images/priority_order.drawio.svg) + +`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. + +##### `efficient_path` + +When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 1 | shift_pull_out | +| ... | ... | +| N | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | geometric_pull_out | + +This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. + +##### `short_back_distance` + +For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | shift_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | shift_pull_out | +| N | geometric_pull_out | + +This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. + +### 2. Collision detection with dynamic obstacles + +- **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) + +- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring there is no overlap with the road lane's centerline. This is to avoid hindering the progress of following vehicles. + +- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline. + +```plantuml +@startuml +start +:Path Generation; + +if (Collision with dynamic objects detected?) then (yes) + if (Before departure?) then (yes) + :Deactivate module decision is registered; + else (no) + if (Can stop within constraints \n && \n no crossing centerline?) then (yes) + :Stop; + else (no) + :Continue with caution; + endif + endif +else (no) +endif + +stop +@enduml +``` + +#### **example of safety check performed range for shift pull out** + +Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint overlaps with the center line of the road lane, the safety check against dynamic objects is disabled.
- ![case2](images/start_from_road_shoulder.drawio.svg){width=1000} -
pull out from the shoulder lane
+ ![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100}
+**As a note, no safety check is performed during backward movements. Safety verification commences at the point where the backward motion ceases.** + +## RTC interface + +The system operates distinctly under manual and auto mode, especially concerning the before the departure and interaction with dynamic obstacles. Below are the specific behaviors for each mode: + +### When approval is required? + +#### Forward driving + +- **Start approval required**: Even if a path is generated, approval is required to initiate movement. If a dynamic object poses a risk, such as an approaching vehicle from behind, candidate paths may be displayed, but approval is necessary for departure. + +#### Backward driving + forward driving + +- **Multiple approvals required**: When the planned path includes a backward driving, multiple approvals are needed before starting the reverse and again before restarting forward movement. Before initiating forward movement, the system conducts safety checks against dynamic obstacles. If a detection is detected, approval for departure is necessary. + +This differentiation ensures that the vehicle operates safely by requiring human intervention in manual mode(`enable_rtc` is true) at critical junctures and incorporating automatic safety checks in both modes to prevent unsafe operations in the presence of dynamic obstacles. + ## Design ```plantuml @@ -79,39 +333,6 @@ PullOutPath --o PullOutPlannerBase | collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | -## Safety check with static obstacles - -1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame) -2. Calculate object's polygon -3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path - -![pull_out_collision_check](./images/pull_out_collision_check.drawio.svg) - -## Safety check with dynamic obstacles - -### **Basic concept of safety check against dynamic obstacles** - -This is based on the concept of RSS. For the logic used, refer to the link below. -See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) - -### **Collision check performed range** - -A collision check with dynamic objects is primarily performed between the shift start point and end point. The range for safety check varies depending on the type of path generated, so it will be explained for each pattern. - -#### **Shift pull out** - -For the "shift pull out", safety verification starts at the beginning of the shift and ends at the shift's conclusion. - -#### **Geometric pull out** - -Since there's a stop at the midpoint during the shift, this becomes the endpoint for safety verification. After stopping, safety verification resumes. - -#### **Backward pull out start point search** - -During backward movement, no safety check is performed. Safety check begins at the point where the backward movement ends. - -![collision_check_range](./images/collision_check_range.drawio.svg) - ### **Ego vehicle's velocity planning** WIP @@ -250,52 +471,6 @@ If a safe path cannot be generated from the current position, search backwards f [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) -### **search priority** - -If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. - -During this backward search, different policies can be applied based on `search_priority` parameters: - -Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. -Opting for `short_back_distance` aims to find a location with the least possible backward movement. - -![priority_order](./images/priority_order.drawio.svg) - -`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. - -#### `efficient_path` - -When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. - -| Index | Planner Type | -| ----- | ------------------ | -| 0 | shift_pull_out | -| 1 | shift_pull_out | -| ... | ... | -| N | shift_pull_out | -| 0 | geometric_pull_out | -| 1 | geometric_pull_out | -| ... | ... | -| N | geometric_pull_out | - -This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. - -#### `short_back_distance` - -For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. - -| Index | Planner Type | -| ----- | ------------------ | -| 0 | shift_pull_out | -| 0 | geometric_pull_out | -| 1 | shift_pull_out | -| 1 | geometric_pull_out | -| ... | ... | -| N | shift_pull_out | -| N | geometric_pull_out | - -This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. - ### **parameters for backward pull out start point search** | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index d174b54b9ccbe..df89427e90fa6 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,15 +5,15 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.5, 1.0] - collision_check_distance_from_end: 1.0 + collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true - check_shift_path_lane_departure: false + check_shift_path_lane_departure: true + shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 lateral_jerk: 0.5 @@ -23,6 +23,7 @@ maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true + geometric_collision_check_distance_from_end: 0.0 divide_pull_out_path: true geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 @@ -32,7 +33,7 @@ # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 30.0 + max_back_distance: 20.0 backward_search_resolution: 2.0 backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 diff --git a/planning/behavior_path_start_planner_module/images/collision_check_range.drawio.svg b/planning/behavior_path_start_planner_module/images/collision_check_range.drawio.svg deleted file mode 100644 index 064f3602d6b6a..0000000000000 --- a/planning/behavior_path_start_planner_module/images/collision_check_range.drawio.svg +++ /dev/null @@ -1,119 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
safety check range
-
- - -
-
no-safety check range
-
- - -
-
safety check start point with vehicle frame
-
- - -
-
safety check end point with vehicle frame
-
- - diff --git a/planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg b/planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg new file mode 100644 index 0000000000000..68c44e2004983 --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
safety check range
+
+ + +
+
no-safety check range
+
+ + +
+
safety check start point with vehicle frame
+
+ + +
+
safety check end point with vehicle frame
+
+ + +
+
overlap between ego vehicle frame and centerline
+
+ + +
+
shift end point
+
+ + diff --git a/planning/behavior_path_start_planner_module/images/freespace_pull_out.png b/planning/behavior_path_start_planner_module/images/freespace_pull_out.png new file mode 100644 index 0000000000000..86b95379f9bc1 Binary files /dev/null and b/planning/behavior_path_start_planner_module/images/freespace_pull_out.png differ diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg new file mode 100644 index 0000000000000..295b5f97c1cf0 --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg new file mode 100644 index 0000000000000..434e7772d975f --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg index ab48c30b6fdae..ec351fbff9e35 100644 --- a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg +++ b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg @@ -5,7 +5,7 @@ width="964px" height="251px" viewBox="-0.5 -0.5 964 251" - content="<mxfile host="0i6112g2df9b6ngu9buda4iblme4t77dtbb7m4umbjihr1cp3ej8" modified="2024-01-01T10:17:58.085Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Code/1.85.1 Chrome/114.0.5735.289 Electron/25.9.7 Safari/537.36" etag="XuiGFtwrEeTmwmuY7TrH" version="12.2.4" pages="1"><diagram id="wUdfcF72OrnvxqYuaHJQ" name="collision_check_range">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</diagram></mxfile>" + content="<mxfile host="0i6112g2df9b6ngu9buda4iblme4t77dtbb7m4umbjihr1cp3ej8" modified="2024-02-06T11:07:08.471Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Code/1.86.0 Chrome/118.0.5993.159 Electron/27.2.3 Safari/537.36" etag="x3Z_KLsXoiAOduD40paA" version="12.2.4" pages="1"><diagram id="wUdfcF72OrnvxqYuaHJQ" name="collision_check_range">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</diagram></mxfile>" > @@ -13,7 +13,6 @@ - @@ -29,9 +28,6 @@ - - - @@ -84,7 +80,7 @@ - +
- -
-
backward_search_resolution
-
- - +
geometric pull out path
 - +
start pose candidate
 - +
idx: 0
 - +
idx: 1
 - +
idx: 2
 - +
idx: 3
 - +
idx: 4
 - +
shift pull out path
 - +
- +
(3, shift_pull_out)
 - +
idx: N
 - +
- +
- +
- +
high priority
 - +
+ + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg new file mode 100644 index 0000000000000..24fe0ccc5f672 --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg new file mode 100644 index 0000000000000..cc7223786833a --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg deleted file mode 100644 index 4a70534d42d2e..0000000000000 --- a/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - diff --git a/planning/behavior_path_start_planner_module/images/start_planner_example.png b/planning/behavior_path_start_planner_module/images/start_planner_example.png new file mode 100644 index 0000000000000..598367c1a6e84 Binary files /dev/null and b/planning/behavior_path_start_planner_module/images/start_planner_example.png differ diff --git a/planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg b/planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg similarity index 55% rename from planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg rename to planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg index 46cdb07150c3a..3a5c2a5cce916 100644 --- a/planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg +++ b/planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg @@ -1,34 +1,148 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + +
+
backward_search_resolution
+
+ + +
+
start pose candidate
+
+ + +
+
idx: 0
+
+ + +
+
idx: 1
+
+ + +
+
idx: 2
+
+ + +
+
idx: 3
+
+ + +
+
idx: 4
+
+ + +
+
shift pull out path
+
+ + +
+
idx: N
+
+ + +
+
max_back_distance
+
+ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 8f8d2baf9c85e..ecd99393eae2b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -46,6 +46,7 @@ struct StartPlannerDebugData std::vector ego_predicted_path; // collision check debug map CollisionCheckDebugMap collision_check; + lanelet::ConstLanelets departure_check_lanes; Pose refined_start_pose; std::vector start_pose_candidates; @@ -63,7 +64,6 @@ struct StartPlannerParameters double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; std::vector collision_check_margins{}; - double collision_check_distance_from_end{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; @@ -71,6 +71,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; double lateral_jerk{0.0}; @@ -80,6 +81,7 @@ struct StartPlannerParameters double deceleration_interval{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; + double geometric_collision_check_distance_from_end; bool divide_pull_out_path{false}; ParallelParkingParameters parallel_parking_parameters{}; // search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index fef9a4aa8ebfc..6bf85a4679dfd 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -50,9 +50,9 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); - void setDrivableLanes(const lanelet::ConstLanelets & drivable_lanes) + void setDepartureCheckLanes(const lanelet::ConstLanelets & departure_check_lanes) { - drivable_lanes_ = drivable_lanes; + departure_check_lanes_ = departure_check_lanes; } std::shared_ptr lane_departure_checker_; @@ -64,7 +64,7 @@ class ShiftPullOut : public PullOutPlannerBase const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; - lanelet::ConstLanelets drivable_lanes_; + lanelet::ConstLanelets departure_check_lanes_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 5bbde1c2fc523..3c5b69037279d 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -35,6 +35,7 @@ #include +#include #include #include #include @@ -83,6 +84,21 @@ class StartPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); + ~StartPlannerModule() + { + if (freespace_planner_timer_) { + freespace_planner_timer_->cancel(); + } + + while (is_freespace_planner_cb_running_.load()) { + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 1000, "Waiting for freespace planner callback to finish..."); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + RCLCPP_INFO_THROTTLE(getLogger(), *clock_, 1000, "freespace planner callback finished"); + } + void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); @@ -135,12 +151,22 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + // Flag class for managing whether a certain callback is running in multi-threading + class ScopedFlag + { + public: + explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } + + ~ScopedFlag() { flag_.store(false); } + + private: + std::atomic & flag_; + }; + bool canTransitSuccessState() override; bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override; - /** * @brief init member variables. */ @@ -164,6 +190,7 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; + bool isOverlapWithCenterLane() const; bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; @@ -174,7 +201,8 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); - PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); @@ -202,6 +230,8 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr freespace_planner_; rclcpp::TimerBase::SharedPtr freespace_planner_timer_; rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_; + std::atomic is_freespace_planner_cb_running_; + // TODO(kosuke55) // Currently, we only do lock when updating a member of status_. // However, we need to ensure that the value does not change when referring to it. @@ -242,8 +272,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - void updateDrivableLanes(); - lanelet::ConstLanelets createDrivableLanes() const; + void updateDepartureCheckLanes(); + lanelet::ConstLanelets createDepartureCheckLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/behavior_path_start_planner_module/package.xml index cbfdba0d07a57..f897188d60444 100644 --- a/planning/behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_start_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 8cc0b34082e44..7fb52d59c418b 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_distance_from_end = - node->declare_parameter(ns + "collision_check_distance_from_end"); 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"); @@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) 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.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 = @@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); // 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"); diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 3b4d08b65923c..8eca8479fbd44 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -88,7 +88,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(drivable_lanes_, transformed_vehicle_footprint); + LaneDepartureChecker::isOutOfLane(departure_check_lanes_, transformed_vehicle_footprint); if (i <= start_segment_idx) { if (!is_out_of_lane) { cropped_path.points.push_back(shift_path.points.at(i)); @@ -100,9 +100,16 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points = cropped_path.points; // check lane departure + // The method for lane departure checking verifies if the footprint of each point on the path is + // contained within a lanelet using `boost::geometry::within`, which incurs a high computational + // cost. + // TODO(someone): improve the method for detecting lane departures without using + // lanelet::ConstLanelets, making it unnecessary to retain departure_check_lanes_ as a member + // variable. if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_shift_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane( + departure_check_lanes_, path_shift_start_to_end)) { continue; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 6c9000caac956..85732fea66dd0 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -21,8 +21,10 @@ #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include +#include #include #include @@ -55,7 +57,8 @@ StartPlannerModule::StartPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + is_freespace_planner_cb_running_{false} { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); @@ -86,6 +89,8 @@ StartPlannerModule::StartPlannerModule( void StartPlannerModule::onFreespacePlannerTimer() { + const ScopedFlag flag(is_freespace_planner_cb_running_); + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -136,7 +141,7 @@ void StartPlannerModule::initVariables() debug_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(debug_data_.collision_check); - updateDrivableLanes(); + updateDepartureCheckLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -211,7 +216,8 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { - return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; + return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && + !isOverlapWithCenterLane(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -278,6 +284,37 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } +bool StartPlannerModule::isOverlapWithCenterLane() const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto current_lanes = utils::getCurrentLanes(planner_data_); + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + for (const auto & current_lane : current_lanes) { + std::vector centerline_points; + for (const auto & point : current_lane.centerline()) { + geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point); + centerline_points.push_back(center_point); + } + + for (size_t i = 0; i < centerline_points.size() - 1; ++i) { + const auto & p1 = centerline_points.at(i); + const auto & p2 = centerline_points.at(i + 1); + for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { + const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); + const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + + if (intersection.has_value()) { + return true; + } + } + } + } + return false; +} + bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -347,11 +384,6 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToRunningState() -{ - return isActivated() && !isWaitingApproval(); -} - BehaviorModuleOutput StartPlannerModule::plan() { if (isWaitingApproval()) { @@ -569,7 +601,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; - updateDrivableLanes(); + updateDepartureCheckLanes(); } void StartPlannerModule::incrementPathIndex() @@ -662,8 +694,8 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - collision_check_margin)) { + vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()), + pull_out_lane_stop_objects, collision_check_margin)) { return false; } @@ -677,8 +709,17 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type) { + const std::map collision_check_distances = { + {behavior_path_planner::PlannerType::SHIFT, + parameters_->shift_collision_check_distance_from_end}, + {behavior_path_planner::PlannerType::GEOMETRIC, + parameters_->geometric_collision_check_distance_from_end}}; + + const double collision_check_distance_from_end = collision_check_distances.at(planner_type); + PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { combined_path.points.insert( @@ -687,7 +728,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat // calculate collision check end idx const size_t collision_check_end_idx = std::invoke([&]() { const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + combined_path.points, path.end_pose.position, collision_check_distance_from_end); if (collision_check_end_pose) { return motion_utils::findNearestIndex( @@ -931,8 +972,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, std::numeric_limits::max()); - // Set the maximum backward distance less than the distance from the vehicle's base_link to the - // lane's rearmost point to prevent lane departure. + // Set the maximum backward distance less than the distance from the vehicle's base_link to + // the lane's rearmost point to prevent lane departure. const double current_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length; const double allowed_backward_distance = std::clamp( @@ -1164,6 +1205,9 @@ bool StartPlannerModule::isSafePath() const // TODO(Sugahara): should safety check for backward path const auto pull_out_path = getCurrentPath(); + if (pull_out_path.points.empty()) { + return false; + } const auto & current_pose = planner_data_->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data_->self_odometry->twist.twist.linear.x, @@ -1207,8 +1251,12 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - behavior_path_planner::updateSafetyCheckDebugData( - debug_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + // debug + { + debug_data_.filtered_objects = filtered_objects; + debug_data_.target_objects_on_lane = target_objects_on_lane; + debug_data_.ego_predicted_path = ego_predicted_path; + } return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, @@ -1220,8 +1268,8 @@ bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; - // Check if the goal and ego are in the same route segment. If not, this is out of scope of this - // function. Return false. + // Check if the goal and ego are in the same route segment. If not, this is out of scope of + // this function. Return false. lanelet::ConstLanelet ego_lanelet; rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet); const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet); @@ -1335,19 +1383,23 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -void StartPlannerModule::updateDrivableLanes() +void StartPlannerModule::updateDepartureCheckLanes() { - const auto drivable_lanes = createDrivableLanes(); + const auto departure_check_lanes = createDepartureCheckLanes(); for (auto & planner : start_planners_) { auto shift_pull_out = std::dynamic_pointer_cast(planner); if (shift_pull_out) { - shift_pull_out->setDrivableLanes(drivable_lanes); + shift_pull_out->setDepartureCheckLanes(departure_check_lanes); } } + // debug + { + debug_data_.departure_check_lanes = departure_check_lanes; + } } -lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const +lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const { const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; @@ -1366,13 +1418,14 @@ lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const [this](const auto & pull_out_lane) { return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); }); - const auto drivable_lanes = utils::transformToLanelets( + const auto departure_check_lanes = utils::transformToLanelets( utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); - return drivable_lanes; + return departure_check_lanes; } void StartPlannerModule::setDebugData() { + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; using marker_utils::createObjectsMarkerArray; @@ -1387,6 +1440,12 @@ void StartPlannerModule::setDebugData() using tier4_autoware_utils::createMarkerScale; using visualization_msgs::msg::Marker; + const auto red_color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + const auto cyan_color = createMarkerColor(0.0, 1.0, 1.0, 0.2); + const auto pink_color = createMarkerColor(1.0, 0.5, 0.5, 0.35); + const auto purple_color = createMarkerColor(1.0, 0.0, 1.0, 0.99); + const auto white_color = createMarkerColor(1.0, 1.0, 1.0, 0.99); + const auto life_time = rclcpp::Duration::from_seconds(1.5); auto add = [&](MarkerArray added) { for (auto & marker : added.markers) { @@ -1407,16 +1466,20 @@ void StartPlannerModule::setDebugData() // visualize collision_check_end_pose and footprint { const auto local_footprint = vehicle_info_.createFootprint(); + std::map collision_check_distances = { + {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end}, + {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; + + double collision_check_distance_from_end = collision_check_distances[status_.planner_type]; const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, - parameters_->collision_check_distance_from_end); + collision_check_distance_from_end); if (collision_check_end_pose) { add(createPoseMarkerArray( *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); - auto marker = tier4_autoware_utils::createDefaultMarker( + auto marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0, - Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + Marker::LINE_LIST, createMarkerScale(0.1, 0.1, 0.1), red_color); const auto footprint = transformVector( local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose)); const double ego_z = planner_data_->self_odometry->pose.pose.position.z; @@ -1436,13 +1499,13 @@ void StartPlannerModule::setDebugData() { MarkerArray start_pose_footprint_marker_array{}; MarkerArray start_pose_text_marker_array{}; - const auto purple = createMarkerColor(1.0, 0.0, 1.0, 0.99); Marker footprint_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates", 0, Marker::LINE_STRIP, - createMarkerScale(0.2, 0.2, 0.2), purple); + createMarkerScale(0.2, 0.2, 0.2), purple_color); Marker text_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates_idx", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), + purple_color); footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); for (size_t i = 0; i < debug_data_.start_pose_candidates.size(); ++i) { @@ -1463,10 +1526,9 @@ void StartPlannerModule::setDebugData() // visualize the footprint from pull_out_start pose to pull_out_end pose along the path { MarkerArray pull_out_path_footprint_marker_array{}; - const auto pink = createMarkerColor(1.0, 0.0, 1.0, 0.99); Marker pull_out_path_footprint_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shift_path_footprint", 0, Marker::LINE_STRIP, - createMarkerScale(0.2, 0.2, 0.2), pink); + createMarkerScale(0.2, 0.2, 0.2), pink_color); pull_out_path_footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); PathWithLaneId path_shift_start_to_end{}; const auto shift_path = status_.pull_out_path.partial_paths.front(); @@ -1524,8 +1586,7 @@ void StartPlannerModule::setDebugData() const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.found_pull_out_path ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.found_pull_out_path ? white_color : red_color; auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); @@ -1540,6 +1601,15 @@ void StartPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } + + add(laneletsAsTriangleMarkerArray( + "departure_check_lanes_for_shift_pull_out_path", debug_data_.departure_check_lanes, + cyan_color)); + + const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + add(laneletsAsTriangleMarkerArray( + "pull_out_lanes_for_static_objects_collision_check", pull_out_lanes, pink_color)); } void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index efbff7e2af51d..54314e50308ff 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -348,7 +348,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) { /* get detection area */ if (turn_direction_ == TurnDirection::INVALID) { @@ -409,6 +409,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( exist_in_right_conflict_area || exist_in_opposite_conflict_area; if (exist_in_detection_area && exist_in_conflict_area) { debug_data_.conflicting_targets.objects.push_back(object); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); return true; } } diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 6f8450568939c..2ca2fe7950989 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -113,7 +113,7 @@ class BlindSpotModule : public SceneModuleInterface lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const; + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose); /** * @brief Create half lanelet diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index ce231659ccf78..c1ddc28e03426 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -196,7 +196,7 @@ In the `stuck_vehicle` namespace, the following parameters are defined. | ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | | `stuck_vehicle_velocity` | [m/s] | double | maximum velocity threshold whether the target vehicle is stopped or not | | `max_stuck_vehicle_lateral_offset` | [m] | double | maximum lateral offset of the target vehicle position | -| `stuck_vehicle_attention_range` | [m] | double | detection area length ahead of the crosswalk | +| `required_clearance` | [m] | double | clearance to be secured between the ego and the ahead vehicle | | `min_acc` | [m/ss] | double | minimum acceleration to stop | | `min_jerk` | [m/sss] | double | minimum jerk to stop | | `max_jerk` | [m/sss] | double | maximum jerk to stop | diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index aa5dfed1e4bc5..29079b99b6718 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -30,7 +30,7 @@ enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. - stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle min_acc: -1.0 # min acceleration [m/ss] min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg index c517be5bb9967..773edf7989de9 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg @@ -1,92 +1,93 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -
+
-
- stuck_vehicle_attention_range +
+ vehicle_length+required_clearance
- stuck_vehicle_atten... + vehicle_length+required_clearance - - - - - - - - - + + + + + + + + + + -
+
-
+
max_stuck_vehicle_lateral_offset
- max_stuck_vehicle_l... + max_stuck_vehicle_lateral_offset - - + + - + Text is not SVG - cannot display diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index dc3ce32be8505..094369b0a7a3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -68,8 +68,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); - cp.stuck_vehicle_attention_range = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.required_clearance = + getOrDeclareParameter(node, ns + ".stuck_vehicle.required_clearance"); cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); cp.max_jerk_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 48ae5ff03b8bd..cee6975df3155 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -906,48 +906,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( continue; } - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pose.position); if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto ego_vel = planner_data_->current_velocity->twist.linear.x; - const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - - const double near_attention_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; - - const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); - - if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { - // Plan STOP considering min_acc, max_jerk and min_jerk. - const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, - p.min_jerk_for_stuck_vehicle); - if (!min_feasible_dist_ego2stop) { - continue; + // check if STOP is required + const double crosswalk_front_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.front(), 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) - + object.shape.dimensions.x / 2.0; + const double required_space_length = + planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance; + + if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) { + // If there exists at least one vehicle ahead, plan STOP considering min_acc, max_jerk and + // min_jerk. Note that nearest search is not required because the stop pose independents to + // the vehicles. + const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle, + p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle); + if (!braking_distance) { + return {}; } + const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); - const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop); const double dist_to_ego = calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); - const auto feasible_stop_pose = calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); if (!feasible_stop_pose) { - continue; + return {}; } - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); - - // early return may not appropriate because the nearest in range object should be handled - return createStopFactor(*feasible_stop_pose, {obj_pos}); + setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED); + return createStopFactor(*feasible_stop_pose, {obj_pose.position}); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 247907ad2dc80..ed1e342df9f7a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -124,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface bool enable_stuck_check_in_intersection{false}; double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; - double stuck_vehicle_attention_range; + double required_clearance; double min_acc_for_stuck_vehicle; double max_jerk_for_stuck_vehicle; double min_jerk_for_stuck_vehicle; diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 4c8fe5c6fa0f5..07847a08c1209 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -12,6 +12,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp src/scene_intersection.cpp src/intersection_lanelets.cpp + src/object_manager.cpp + src/decision_result.cpp src/scene_intersection_prepare_data.cpp src/scene_intersection_stuck.cpp src/scene_intersection_occlusion.cpp diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 108e021240851..c7f6f62d575a0 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -59,6 +59,8 @@ object_expected_deceleration: 2.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 + avoid_collision_by_acceleration: + object_time_margin_to_collision_point: 4.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0c9b3407d0f38..0bed7d32f412f 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -22,10 +22,12 @@ autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common + fmt geometry_msgs interpolation lanelet2_extension libopencv-dev + magic_enum motion_utils nav_msgs pluginlib @@ -33,7 +35,6 @@ route_handler rtc_interface tf2_geometry_msgs - tier4_api_msgs tier4_autoware_utils tier4_planning_msgs vehicle_info_util diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 350f6d774f7cf..b323cdf4cb5da 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -31,8 +31,6 @@ #include #include -namespace behavior_velocity_planner -{ namespace { using tier4_autoware_utils::appendMarkerArray; @@ -40,14 +38,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(lane_id); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -113,7 +111,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( return msg; } -visualization_msgs::msg::MarkerArray createLineMarkerArray( +visualization_msgs::msg::MarkerArray createArrowLineMarkerArray( const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end, const std::string & ns, const int64_t id, const double r, const double g, const double b) { @@ -139,6 +137,28 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( return msg; } +visualization_msgs::msg::MarkerArray createLineMarkerArray( + const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end, + const std::string & ns, const int64_t id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = ns + "_line"; + marker.id = id; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.1; + marker.color = createMarkerColor(r, g, b, 0.999); + marker.points.push_back(point_start); + marker.points.push_back(point_end); + + msg.markers.push_back(marker); + return msg; +} + [[maybe_unused]] visualization_msgs::msg::Marker createPointMarkerArray( const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r, const double g, const double b) @@ -158,8 +178,59 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( return marker_point; } +constexpr std::tuple white() +{ + constexpr uint64_t code = 0xfdfdfd; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple green() +{ + constexpr uint64_t code = 0x5fa641; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple yellow() +{ + constexpr uint64_t code = 0xebce2b; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple red() +{ + constexpr uint64_t code = 0xba1c30; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple light_blue() +{ + constexpr uint64_t code = 0x96cde6; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} } // namespace +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; @@ -168,14 +239,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.attention_area.value(), "attention_area", lane_id_, 0.0, 1.0, 0.0), &debug_marker_array); } if (debug_data_.occlusion_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, 0.568, 0.596), &debug_marker_array); @@ -183,14 +254,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.adjacent_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array); } if (debug_data_.first_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -198,7 +269,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.second_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -214,7 +285,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.yield_stuck_detect_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, 0.34509, 0.6588235), &debug_marker_array); @@ -222,7 +293,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.ego_lane) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.ego_lane.value()}, "ego_lane", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); } @@ -235,59 +306,58 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - size_t i{0}; - for (const auto & p : debug_data_.candidate_collision_object_polygons) { - appendMarkerArray( - debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, - 0.5), - &debug_marker_array, now); - } - + static constexpr auto white = ::white(); + static constexpr auto green = ::green(); + static constexpr auto yellow = ::yellow(); + static constexpr auto red = ::red(); + static constexpr auto light_blue = ::light_blue(); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", + module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), + std::get<1>(green), std::get<2>(green)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, - 0.0, 1.0, 0.0), + debug_data_.misjudge_targets, "misjudge_targets", module_id_, now, std::get<0>(yellow), + std::get<1>(yellow), std::get<2>(yellow)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), + debug_data_.too_late_detect_targets, "too_late_detect_targets", module_id_, now, + std::get<0>(red), std::get<1>(red), std::get<2>(red)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + debug_data_.parked_targets, "parked_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, - 0.99, 0.6), + debug_data_.stuck_targets, "stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - /* appendMarkerArray( - createPoseMarkerArray( - debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "yield_stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - */ if (debug_data_.first_pass_judge_wall_pose) { const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -297,7 +367,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -314,10 +384,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.nearest_occlusion_projection) { const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); appendMarkerArray( - createLineMarkerArray( + ::createArrowLineMarkerArray( point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), &debug_marker_array, now); } + + if (debug_data_.traffic_light_observation) { + const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; + const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; + + const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value(); + geometry_msgs::msg::Point tl_point_point; + tl_point_point.x = tl_point.x(); + tl_point_point.y = tl_point.y(); + tl_point_point.z = tl_point.z(); + const auto tl_color = (color == GREEN) ? green : (color == YELLOW ? yellow : red); + const auto [r, g, b] = tl_color; + appendMarkerArray( + ::createLineMarkerArray( + ego.position, tl_point_point, "intersection_traffic_light", lane_id_, r, g, b), + &debug_marker_array, now); + } return debug_marker_array; } @@ -369,11 +456,11 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = planning_utils::bitShift(module_id_); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( - createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), + ::createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp new file mode 100644 index 0000000000000..aefd59a72f9b4 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "decision_result.hpp" + +namespace behavior_velocity_planner::intersection +{ +std::string formatDecisionResult(const DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "InternalError because " + state.error; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + + state.evasive_report; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "YieldStuckStop:\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "OccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "FullyPrioritized\nsafety_report:" + state.safety_report; + } + return ""; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp index 48b0ecf1349a5..da71168c2c4ca 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -23,16 +23,23 @@ namespace behavior_velocity_planner::intersection { /** - * @struct - * @brief Internal error or ego already passed pass_judge_line + * @brief internal error */ -struct Indecisive +struct InternalError { std::string error; }; /** - * @struct + * @brief + */ +struct OverPassJudge +{ + std::string safety_report; + std::string evasive_report; +}; + +/** * @brief detected stuck vehicle */ struct StuckStop @@ -43,17 +50,16 @@ struct StuckStop }; /** - * @struct * @brief yielded by vehicle on the attention area */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief only collision is detected */ struct NonOccludedCollisionStop @@ -61,10 +67,10 @@ struct NonOccludedCollisionStop size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief occlusion is detected so ego needs to stop at the default stop line position */ struct FirstWaitBeforeOcclusion @@ -76,7 +82,6 @@ struct FirstWaitBeforeOcclusion }; /** - * @struct * @brief ego is approaching the boundary of attention area in the presence of traffic light */ struct PeekingTowardOcclusion @@ -96,7 +101,6 @@ struct PeekingTowardOcclusion }; /** - * @struct * @brief both collision and occlusion are detected in the presence of traffic light */ struct OccludedCollisionStop @@ -110,10 +114,10 @@ struct OccludedCollisionStop //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; + std::string safety_report; }; /** - * @struct * @brief at least occlusion is detected in the absence of traffic light */ struct OccludedAbsenceTrafficLight @@ -124,10 +128,10 @@ struct OccludedAbsenceTrafficLight size_t closest_idx{0}; size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; + std::string safety_report; }; /** - * @struct * @brief both collision and occlusion are not detected */ struct Safe @@ -138,7 +142,6 @@ struct Safe }; /** - * @struct * @brief traffic light is red or arrow signal */ struct FullyPrioritized @@ -147,10 +150,12 @@ struct FullyPrioritized size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line + InternalError, //! internal process error, or over the pass judge line + OverPassJudge, //! over the pass judge lines StuckStop, //! detected stuck vehicle YieldStuckStop, //! detected yield stuck vehicle NonOccludedCollisionStop, //! detected collision while FOV is clear @@ -162,41 +167,7 @@ using DecisionResult = std::variant< FullyPrioritized //! only detect vehicles violating traffic rules >; -inline std::string formatDecisionResult(const DecisionResult & decision_result) -{ - if (std::holds_alternative(decision_result)) { - const auto indecisive = std::get(decision_result); - return "Indecisive because " + indecisive.error; - } - if (std::holds_alternative(decision_result)) { - return "StuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; - } - if (std::holds_alternative(decision_result)) { - return "Safe"; - } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; - } - return ""; -} +std::string formatDecisionResult(const DecisionResult & decision_result); } // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index c47f016fbdbda..9002c88354d68 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -27,7 +27,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief wrapper class of interpolated path with lane id */ struct InterpolatedPathInfo diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 11deae6bdc001..9624d375de122 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -31,7 +31,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionLanelets */ struct IntersectionLanelets @@ -174,7 +173,6 @@ struct IntersectionLanelets }; /** - * @struct * @brief see the document for more details of PathLanelets */ struct PathLanelets diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 64d20b81e3fad..99d79d4468b38 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -21,7 +21,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionStopLines */ struct IntersectionStopLines @@ -63,9 +62,9 @@ struct IntersectionStopLines /** * second_pass_judge_line is before second_attention_stopline by the braking distance. if - * second_attention_lane is null, it is same as first_pass_judge_line + * second_attention_lane is null, it is null */ - size_t second_pass_judge_line{0}; + std::optional second_pass_judge_line{std::nullopt}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 615991bc5e027..8ab67c810a30e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -133,6 +133,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + ip.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point = getOrDeclareParameter( + node, + ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp new file mode 100644 index 0000000000000..fe327704f61c2 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -0,0 +1,309 @@ +// 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 "object_manager.hpp" + +#include +#include +#include // for toPolygon2d + +#include +#include +#include + +#include +#include + +#include + +namespace +{ +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} + +tier4_autoware_utils::Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + namespace bg = boost::geometry; + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + tier4_autoware_utils::Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + tier4_autoware_utils::Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} + +} // namespace + +namespace behavior_velocity_planner::intersection +{ +namespace bg = boost::geometry; + +ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_str(::to_string(uuid)) +{ +} + +void ObjectInfo::initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + std::optional attention_lanelet_opt_, + std::optional stopline_opt_) +{ + predicted_object_ = object; + attention_lanelet_opt = attention_lanelet_opt_; + stopline_opt = stopline_opt_; + unsafe_interval_ = std::nullopt; + calc_dist_to_stopline(); +} + +void ObjectInfo::update_safety( + const std::optional & unsafe_interval, + const std::optional & safe_interval, const bool safe_under_traffic_control) +{ + unsafe_interval_ = unsafe_interval; + safe_interval_ = safe_interval; + safe_under_traffic_control_ = safe_under_traffic_control; +} + +std::optional ObjectInfo::estimated_past_position( + const double past_duration) const +{ + if (!attention_lanelet_opt) { + return std::nullopt; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto current_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto distance = current_arc_coords.distance; + const auto past_length = + current_arc_coords.length - + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x * past_duration; + const auto past_point = lanelet::geometry::fromArcCoordinates( + attention_lanelet.centerline2d(), lanelet::ArcCoordinates{past_length, distance}); + geometry_msgs::msg::Point past_position; + past_position.x = past_point.x(); + past_position.y = past_point.y(); + return std::make_optional(past_position); +} + +void ObjectInfo::calc_dist_to_stopline() +{ + if (!stopline_opt || !attention_lanelet_opt) { + return; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto stopline = stopline_opt.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0; + stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0; + stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center); + dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length); +} + +bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + return dist_to_stopline > braking_distance; +} + +bool ObjectInfo::can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const +{ + if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto stopline = stopline_opt.value(); + const auto stopline_p1 = stopline.front(); + const auto stopline_p2 = stopline.back(); + const tier4_autoware_utils::Point2d stopline_mid{ + (stopline_p1.x() + stopline_p2.x()) / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; + const auto attention_lane_end = attention_lanelet.centerline().back(); + const tier4_autoware_utils::LineString2d attention_lane_later_part( + {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + tier4_autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; + bg::intersection( + attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); + if (ego_collision_points.empty()) { + return false; + } + const auto expected_collision_point = ego_collision_points.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; + const double stopline_to_ego_path = std::hypot( + expected_collision_point.x() - stopline_mid.x(), + expected_collision_point.y() - stopline_mid.y()); + const double object_to_ego_path = stopline_to_ego_path - stopline_to_object; + // NOTE: if object_to_ego_path < 0, object passed ego path + return object_to_ego_path > tolerable_overshoot; +} + +bool ObjectInfo::before_stopline_by(const double margin) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + return dist_to_stopline < margin; +} + +std::shared_ptr ObjectInfoManager::registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle) +{ + if (objects_info_.count(uuid) == 0) { + auto object = std::make_shared(uuid); + objects_info_[uuid] = object; + } + auto object = objects_info_[uuid]; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } + return object; +} + +void ObjectInfoManager::registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle, + std::shared_ptr object) +{ + objects_info_[uuid] = object; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } +} + +void ObjectInfoManager::clearObjects() +{ + objects_info_.clear(); + attention_area_objects_.clear(); + intersection_area_objects_.clear(); + parked_objects_.clear(); +}; + +std::vector> ObjectInfoManager::allObjects() +{ + std::vector> all_objects = attention_area_objects_; + all_objects.insert( + all_objects.end(), intersection_area_objects_.begin(), intersection_area_objects_.end()); + all_objects.insert(all_objects.end(), parked_objects_.begin(), parked_objects_.end()); + return all_objects; +} + +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt) +{ + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (first_itr == predicted_path.path.cend()) { + // even the predicted path end does not collide with the beginning of ego_lane_poly + return std::nullopt; + } + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (last_itr == predicted_path.path.crend()) { + // even the predicted path start does not collide with the end of ego_lane_poly + return std::nullopt; + } + + const size_t enter_idx = static_cast(first_itr - predicted_path.path.begin()); + const double object_enter_time = + static_cast(enter_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const size_t exit_idx = std::distance(predicted_path.path.begin(), last_itr.base()) - 1; + const double object_exit_time = + static_cast(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const auto lane_position = [&]() { + if (first_attention_lane_opt) { + if (lanelet::geometry::inside( + first_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::FIRST; + } + } + if (second_attention_lane_opt) { + if (lanelet::geometry::inside( + second_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::SECOND; + } + } + return intersection::CollisionInterval::LanePosition::ELSE; + }(); + + std::vector path; + for (const auto & pose : predicted_path.path) { + path.push_back(pose); + } + return intersection::CollisionInterval{ + lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp new file mode 100644 index 0000000000000..e77849570cda8 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -0,0 +1,294 @@ +// 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 OBJECT_MANAGER_HPP_ +#define OBJECT_MANAGER_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash +{ + size_t operator()(const unique_identifier_msgs::msg::UUID & uid) const + { + const auto & ids = uid.uuid; + boost::uuids::uuid u = {ids[0], ids[1], ids[2], ids[3], ids[4], ids[5], ids[6], ids[7], + ids[8], ids[9], ids[10], ids[11], ids[12], ids[13], ids[14], ids[15]}; + return boost::hash()(u); + } +}; +} // namespace std + +namespace behavior_velocity_planner::intersection +{ + +/** + * @brief store collision information + */ +struct CollisionInterval +{ + enum LanePosition { + FIRST, + SECOND, + ELSE, + }; + LanePosition lane_position{LanePosition::ELSE}; + + //! original predicted path + std::vector path; + + //! possible collision interval position index on path + std::pair interval_position; + + //! possible collision interval time(without TTC margin) + std::pair interval_time; +}; + +struct CollisionKnowledge +{ + //! the time when the expected collision is judged + rclcpp::Time stamp; + + enum SafeType { + UNSAFE, + SAFE, + SAFE_UNDER_TRAFFIC_CONTROL, + }; + SafeType safe_type{SafeType::UNSAFE}; + + //! if !safe, this has value, and it safe, this maybe null if the predicted path does not + //! intersect with ego path + std::optional interval{std::nullopt}; + + double observed_velocity; +}; + +/** + * @brief store collision information of object on the attention area + */ +class ObjectInfo +{ +public: + explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); + + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + { + return predicted_object_; + }; + + std::optional is_unsafe() const + { + if (safe_under_traffic_control_) { + return std::nullopt; + } + if (!unsafe_interval_) { + return std::nullopt; + } + return unsafe_interval_; + } + + bool is_safe_under_traffic_control() const { return safe_under_traffic_control_; } + + /** + * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline + */ + void initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + std::optional attention_lanelet_opt, + std::optional stopline_opt); + + /** + * @brief update unsafe_knowledge + */ + void update_safety( + const std::optional & unsafe_interval_opt, + const std::optional & safe_interval_opt, + const bool safe_under_traffic_control); + + /** + * @brief find the estimated position of the object in the past + */ + std::optional estimated_past_position( + const double past_duration) const; + + /** + * @brief check if object can stop before stopline under the deceleration. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_stopline(const double brake_deceleration) const; + + /** + * @brief check if object can stop before stopline within the overshoot margin. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const; + + /** + * @brief check if the object is before the stopline within the specified margin + */ + bool before_stopline_by(const double margin) const; + + void setDecisionAt1stPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_1st_pass_judge_line_passage_ = knowledge; + } + + void setDecisionAt2ndPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_2nd_pass_judge_line_passage_ = knowledge; + } + + const std::optional & unsafe_interval() const { return unsafe_interval_; } + + double observed_velocity() const + { + return predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + } + + const std::optional & decision_at_1st_pass_judge_line_passage() const + { + return decision_at_1st_pass_judge_line_passage_; + } + + const std::optional & decision_at_2nd_pass_judge_line_passage() const + { + return decision_at_2nd_pass_judge_line_passage_; + } + + const std::string uuid_str; + +private: + autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + + //! null if the object in intersection_area but not in attention_area + std::optional attention_lanelet_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional stopline_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional dist_to_stopline_opt{std::nullopt}; + + //! store the information if judged as UNSAFE + std::optional unsafe_interval_{std::nullopt}; + + //! store the information if judged as SAFE + std::optional safe_interval_{std::nullopt}; + + //! true if the object is judged as negligible given traffic light color + bool safe_under_traffic_control_{false}; + + std::optional decision_at_1st_pass_judge_line_passage_{std::nullopt}; + std::optional decision_at_2nd_pass_judge_line_passage_{std::nullopt}; + + /** + * @brief calculate/update the distance to corresponding stopline + */ + void calc_dist_to_stopline(); +}; + +/** + * @brief store predicted objects for intersection + */ +class ObjectInfoManager +{ +public: + std::shared_ptr registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked); + + void registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked, + std::shared_ptr object); + + void clearObjects(); + + const std::vector> & attentionObjects() const + { + return attention_area_objects_; + } + + const std::vector> & parkedObjects() const { return parked_objects_; } + + std::vector> allObjects(); + + const std::unordered_map> & + getObjectsMap() + { + return objects_info_; + } + + void setPassed1stPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_1st_judge_line_first_time_ = time; + } + void setPassed2ndPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_2nd_judge_line_first_time_ = time; + } + +private: + std::unordered_map> objects_info_; + + //! belong to attention area + std::vector> attention_area_objects_; + + //! does not belong to attention area but to intersection area + std::vector> intersection_area_objects_; + + //! parked objects on attention_area/intersection_area + std::vector> parked_objects_; + + std::optional passed_1st_judge_line_first_time_{std::nullopt}; + std::optional passed_2nd_judge_line_first_time_{std::nullopt}; +}; + +/** + * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically + */ +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt); + +} // namespace behavior_velocity_planner::intersection + +#endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp index cc6ad880b8153..5d82183cee2fb 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -15,6 +15,7 @@ #ifndef RESULT_HPP_ #define RESULT_HPP_ +#include #include namespace behavior_velocity_planner::intersection @@ -23,10 +24,6 @@ namespace behavior_velocity_planner::intersection template class Result { -public: - static Result make_ok(const Ok & ok) { return Result(ok); } - static Result make_err(const Error & err) { return Result(err); } - public: explicit Result(const Ok & ok) : data_(ok) {} explicit Result(const Error & err) : data_(err) {} @@ -39,6 +36,18 @@ class Result std::variant data_; }; +template +Result make_ok(Args &&... args) +{ + return Result(Ok{std::forward(args)...}); +} + +template +Result make_err(Args &&... args) +{ + return Result(Error{std::forward(args)...}); +} + } // namespace behavior_velocity_planner::intersection #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 90242dc3edd7e..293103b078450 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,6 +18,7 @@ #include // for toGeomPoly #include +#include #include #include #include // for toPolygon2d @@ -40,6 +41,10 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, @@ -95,10 +100,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - // set default RTC initializeRTCStatus(); - // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); prev_decision_result_ = decision_result; @@ -112,7 +115,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * reactRTCApproval(decision_result, path, stop_reason); - RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } @@ -141,133 +143,145 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - - // utility functions - auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); - }; - auto stoppedForDuration = - [&](const size_t pos, const double duration, StateMachine & state_machine) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < 0.0); - const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { - state_machine.setState(StateMachine::State::GO); - } - return state_machine.getState() == StateMachine::State::GO; - }; - auto stoppedAtPosition = [&](const size_t pos, const double duration) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; - }; - + // ========================================================================================== + // stuck detection + // // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation + // ========================================================================================== const auto is_stuck_status = isStuckStatus(*path, intersection_stoplines, path_lanelets); if (is_stuck_status) { return is_stuck_status.value(); } + // ========================================================================================== + // basic data validation + // // if attention area is empty, collision/occlusion detection is impossible + // + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line, so ego is already in the middle of the intersection, or the + // end of the ego path has just entered the entry of this intersection + // + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, ego has already passed the intersection, or the end of the + // ego path has just entered the entry of this intersection + // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::Indecisive{"attention area is empty"}; + return intersection::InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::Indecisive{"default stop line is null"}; + return intersection::InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::Indecisive{"occlusion stop line is null"}; + return intersection::InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + // ========================================================================================== + // classify the objects to attention_area/intersection_area and update their position, velocity, + // belonging attention lanelet, distance to corresponding stopline + // ========================================================================================== + updateObjectInfoManagerArea(); + + // ========================================================================================== + // occlusion_status is type of occlusion, + // is_occlusion_cleared_with_margin indicates if occlusion is physically detected + // is_occlusion_state indicates if occlusion is detected. OR occlusion is not detected but RTC for + // intersection_occlusion is disapproved, which means ego is virtually occluded + // + // so is_occlusion_cleared_with_margin should be sent to RTC as module decision + // and is_occlusion_status should be only used to decide ego action + // !is_occlusion_state == !physically_occluded && !externally_occluded, so even if occlusion is + // not detected but not approved, SAFE is not sent. + // ========================================================================================== + const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = + getOcclusionStatus(traffic_prioritized_level, interpolated_path_info); - // get intersection area - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const auto is_yield_stuck_status = - isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines, target_objects); - if (is_yield_stuck_status) { - return is_yield_stuck_status.value(); + const auto + [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line] = + isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); + + // ========================================================================================== + // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the + // exit of intersection + // ========================================================================================== + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = + calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); + + // ========================================================================================== + // run collision checking for each objects considering traffic light level. Also if ego just + // passed each pass judge line for the first time, save current collision status for late + // diagnosis + // ========================================================================================== + updateObjectInfoManagerCollision( + path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line); + + const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] = + detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); + const std::string safety_diag = + generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects); + if (is_permanent_go_) { + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{safety_diag, evasive_diag}; + } + return intersection::OverPassJudge{ + "no collision is detected", "ego can safely pass the intersection at this rate"}; } - const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = - getOcclusionStatus( - traffic_prioritized_level, interpolated_path_info, intersection_lanelets, target_objects); - - // TODO(Mamoru Sobue): this should be called later for safety diagnosis - const auto is_over_pass_judge_lines_status = - isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); - if (is_over_pass_judge_lines_status) { - return is_over_pass_judge_lines_status.ok(); + const bool collision_on_1st_attention_lane = + has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + // ========================================================================================== + // this state is very dangerous because ego is very close/over the boundary of 1st attention lane + // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this + // case, possible another collision may be expected on the 2nd attention lane too. + // ========================================================================================== + std::string safety_report = safety_diag; + if ( + is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line && + is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { + safety_report += + "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st " + "attention lane, which is dangerous."; } - [[maybe_unused]] const auto [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line] = - is_over_pass_judge_lines_status.err(); - const auto & current_pose = planner_data_->current_odometry->pose; - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto closest_idx = intersection_stoplines.closest_idx; + const bool is_over_default_stopline = util::isOverTargetIndex( + *path, closest_idx, planner_data_->current_odometry->pose, default_stopline_idx); const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto is_green_pseudo_collision_status = isGreenPseudoCollisionStatus( - *path, collision_stopline_idx, intersection_stoplines, target_objects); + // ========================================================================================== + // pseudo collision detection on green light + // ========================================================================================== + const auto is_green_pseudo_collision_status = + isGreenPseudoCollisionStatus(closest_idx, collision_stopline_idx, intersection_stoplines); if (is_green_pseudo_collision_status) { return is_green_pseudo_collision_status.value(); } - // if ego is waiting for collision detection, the entry time into the intersection is a bit - // delayed for the chattering hold - const bool is_go_out = (activated_ && occlusion_activated_); - const double time_to_restart = - (is_go_out || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd - // pass judge line respectively, ego should go - const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; - const auto last_intersection_stopline_candidate_idx = - second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - *path, closest_idx, last_intersection_stopline_candidate_idx, time_to_restart, - &ego_ttc_time_array); + // ========================================================================================== + // yield stuck detection + // ========================================================================================== + const auto is_yield_stuck_status = + isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); + if (is_yield_stuck_status) { + auto yield_stuck = is_yield_stuck_status.value(); + yield_stuck.safety_report = safety_report; + return yield_stuck; + } - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, - time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -276,7 +290,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (is_prioritized) { return intersection::FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, + safety_report}; } // Safe @@ -286,18 +301,51 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // Only collision if (!is_occlusion_state && has_collision_with_margin) { return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; + const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; const bool stopped_at_default_line = stoppedForDuration( default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, before_creep_state_machine_); if (stopped_at_default_line) { + // ========================================================================================== // if specified the parameter occlusion.temporal_stop_before_attention_area OR // has_no_traffic_light_, ego will temporarily stop before entering attention area + // ========================================================================================== const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -307,8 +355,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::Indecisive{ - "already passed maximum peeking line in the absence of traffic light"}; + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light.\n" + + safety_report, + evasive_diag}; + } + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light safely", + "no evasive action required"}; } return intersection::OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, @@ -316,10 +374,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; + occlusion_wo_tl_pass_judge_line_idx, + safety_diag}; } + + // ========================================================================================== // following remaining block is "has_traffic_light_" + // // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + // ========================================================================================== const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; const bool is_stuck_by_static_occlusion = stoppedAtPosition( @@ -355,7 +418,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_stopline_idx, first_attention_stopline_idx, occlusion_stopline_idx, - static_occlusion_timeout}; + static_occlusion_timeout, + safety_report}; } else { return intersection::PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, @@ -398,7 +462,17 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::Indecisive & result, + [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + return; +} + +template <> +void prepareRTCByDecisionResult( + [[maybe_unused]] const intersection::OverPassJudge & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -606,7 +680,22 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::Indecisive & decision_result, + [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + [[maybe_unused]] const double baselink2front, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason, + [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] IntersectionModule::DebugData * debug_data) +{ + return; +} + +template <> +void reactRTCApprovalByDecisionResult( + [[maybe_unused]] const bool rtc_default_approved, + [[maybe_unused]] const bool rtc_occlusion_approved, + [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, @@ -640,16 +729,14 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if ( - !rtc_occlusion_approved && decision_result.occlusion_stopline_idx && - planner_param.occlusion.enable) { + if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { const auto occlusion_stopline_idx = decision_result.occlusion_stopline_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -689,7 +776,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -726,7 +813,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -769,7 +856,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { if (planner_param.occlusion.creep_during_peeking.enable) { const size_t occlusion_peeking_stopline = decision_result.occlusion_stopline_idx; const size_t closest_idx = decision_result.closest_idx; @@ -807,8 +894,7 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx @@ -878,7 +964,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx : decision_result.occlusion_stopline_idx; @@ -979,7 +1065,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -1023,7 +1109,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -1058,25 +1144,11 @@ void IntersectionModule::reactRTCApproval( bool IntersectionModule::isGreenSolidOn() const { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return false; - } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); - if (!tl_info_opt) { - // the info of this traffic light is not available + if (!last_tl_valid_observation_) { return false; } - const auto & tl_info = tl_info_opt.value(); + const auto & tl_info = last_tl_valid_observation_.value(); for (auto && tl_light : tl_info.signal.elements) { if ( tl_light.color == TrafficSignalElement::GREEN && @@ -1091,118 +1163,91 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); + auto corresponding_arrow = [&](const TrafficSignalElement & element) { + if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) { + return true; + } + if (turn_direction_ == "left" && element.shape == TrafficSignalElement::LEFT_ARROW) { + return true; + } + if (turn_direction_ == "right" && element.shape == TrafficSignalElement::RIGHT_ARROW) { + return true; + } + return false; + }; - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); - if (!tl_info_opt) { - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_opt.value(); - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; + // ========================================================================================== + // if no traffic light information has been available, it is UNKNOWN state which is treated as + // NOT_PRIORITIZED + // + // if there has been any information available in the past more than once, the last valid + // information is kept and used for planning + // ========================================================================================== + auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED; + if (last_tl_valid_observation_) { + auto color = TrafficSignalElement::GREEN; + const auto & tl_info = last_tl_valid_observation_.value(); + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::AMBER && + tl_light.shape == TrafficSignalElement::CIRCLE) { + has_amber_signal = true; + } + if ( + (tl_light.color == TrafficSignalElement::RED && + tl_light.shape == TrafficSignalElement::CIRCLE) || + (tl_light.color == TrafficSignalElement::GREEN && corresponding_arrow(tl_light))) { + // NOTE: Return here since the red signal has the highest priority. + level = TrafficPrioritizedLevel::FULLY_PRIORITIZED; + color = TrafficSignalElement::RED; + break; + } } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; + if (has_amber_signal) { + level = TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + color = TrafficSignalElement::AMBER; + } + if (tl_id_and_point_) { + const auto [tl_id, point] = tl_id_and_point_.value(); + debug_data_.traffic_light_observation = + std::make_tuple(planner_data_->current_odometry->pose, point, tl_id, color); } } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; + return level; } -TargetObjects IntersectionModule::generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const +void IntersectionModule::updateTrafficSignalObservation() { - const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects - TargetObjects target_objects; - target_objects.header = objects_ptr->header; - const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - for (const auto & object : objects_ptr->objects) { - // ignore non-vehicle type objects, such as pedestrian. - if (!isTargetCollisionVehicleType(object)) { - continue; - } - - // check direction of objects - const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = - checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); - if (belong_adjacent_lanelet_id) { - continue; - } + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto is_parked_vehicle = - std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; - auto & container = is_parked_vehicle ? target_objects.parked_attention_objects - : target_objects.attention_objects; - if (intersection_area) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - if (belong_attention_lanelet_id) { - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = std::nullopt; - target_object.stopline = std::nullopt; - target_objects.intersection_area_objects.push_back(target_object); + if (!tl_id_and_point_) { + for (auto && tl_reg_elem : + lane.regulatoryElementsAs()) { + for (const auto & ls : tl_reg_elem->lightBulbs()) { + if (ls.hasAttribute("traffic_light_id")) { + tl_id_and_point_ = std::make_pair(tl_reg_elem->id(), ls.front()); + break; + } } - } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( - object_direction, attention_lanelets, is_parked_vehicle); - belong_attention_lanelet_id.has_value()) { - // intersection_area is not available, use detection_area_with_margin as before - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); } } - for (const auto & object : target_objects.attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all_attention_objects.push_back(object); + if (!tl_id_and_point_) { + // this lane has no traffic light + return; } - for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stopline(); + const auto [tl_id, point] = tl_id_and_point_.value(); + const auto tl_info_opt = + planner_data_->getTrafficSignal(tl_id, true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { + // the info of this traffic light is not available + return; } - return target_objects; + last_tl_valid_observation_ = tl_info_opt.value(); } -intersection::Result< - intersection::Indecisive, - std::pair> -IntersectionModule::isOverPassJudgeLinesStatus( +IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { @@ -1216,8 +1261,11 @@ IntersectionModule::isOverPassJudgeLinesStatus( const size_t pass_judge_line_idx = [&]() { if (planner_param_.occlusion.enable) { if (has_traffic_light_) { + // ========================================================================================== // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its - // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + // position is changed to occlusion_stopline_idx. even if occlusion is cleared by peeking, + // its position should be occlusion_stopline_idx as before + // ========================================================================================== if (passed_1st_judge_line_while_peeking_) { return occlusion_stopline_idx; } @@ -1226,167 +1274,108 @@ IntersectionModule::isOverPassJudgeLinesStatus( if (is_occlusion_state && is_over_first_pass_judge_line) { passed_1st_judge_line_while_peeking_ = true; return occlusion_stopline_idx; - } else { - return first_pass_judge_line_idx; } + // ========================================================================================== + // Otherwise it is first_pass_judge_line + // ========================================================================================== + return first_pass_judge_line_idx; } else if (is_occlusion_state) { + // ========================================================================================== // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area + // ========================================================================================== return occlusion_wo_tl_pass_judge_line_idx; } else { + // ========================================================================================== // if there is no traffic light and occlusion is not detected, pass_judge position is - // default + // default position + // ========================================================================================== return first_pass_judge_line_idx; } } return first_pass_judge_line_idx; }(); - const bool was_safe = std::holds_alternative(prev_decision_result_); + // ========================================================================================== + // at intersection without traffic light, this module ignores occlusion even if occlusion is + // detected for real, so if collision is not detected in that context, that should be interpreted + // as "was_safe" + // ========================================================================================== + const bool was_safe = [&]() { + if (std::holds_alternative(prev_decision_result_)) { + return true; + } + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = + std::get(prev_decision_result_); + return !state.collision_detected; + } + return false; + }(); const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); + bool safely_passed_1st_judge_line_first_time = false; if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { - safely_passed_1st_judge_line_time_ = clock_->now(); + safely_passed_1st_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_1st_judge_line_first_time = true; } const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.first_pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, path); debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); - const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; - const bool is_over_2nd_pass_judge_line = - util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx); - if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { - safely_passed_2nd_judge_line_time_ = clock_->now(); + const auto second_pass_judge_line_idx_opt = intersection_stoplines.second_pass_judge_line; + const std::optional is_over_2nd_pass_judge_line = + second_pass_judge_line_idx_opt + ? std::make_optional(util::isOverTargetIndex( + path, closest_idx, current_pose, second_pass_judge_line_idx_opt.value())) + : std::nullopt; + bool safely_passed_2nd_judge_line_first_time = false; + if ( + is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && was_safe && + !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_2nd_judge_line_first_time = true; + } + if (second_pass_judge_line_idx_opt) { + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx_opt.value(), baselink2front, path); } - debug_data_.second_pass_judge_wall_pose = - planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path); debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); const bool is_over_default_stopline = util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); + + const bool over_default_stopline_for_pass_judge = + is_over_default_stopline || planner_param_.common.enable_pass_judge_before_default_stopline; + const bool over_pass_judge_line_overall = + is_over_2nd_pass_judge_line ? is_over_2nd_pass_judge_line.value() : is_over_1st_pass_judge_line; if ( - ((is_over_default_stopline || - planner_param_.common.enable_pass_judge_before_default_stopline) && - is_over_2nd_pass_judge_line && was_safe) || + (over_default_stopline_for_pass_judge && over_pass_judge_line_overall && was_safe) || is_permanent_go_) { - /* - * This body is active if ego is - * - over the default stopline AND - * - over the 1st && 2nd pass judge line AND - * - previously safe - * , - * which means ego can stop even if it is over the 1st pass judge line but - * - before default stopline OR - * - before the 2nd pass judge line OR - * - or previously unsafe - * . - * In order for ego to continue peeking or collision detection when occlusion is detected after - * ego passed the 1st pass judge line, it needs to be - * - before the default stopline OR - * - before the 2nd pass judge line OR - * - previously unsafe - */ + // ========================================================================================== + // this body is active if ego is + // - over the default stopline AND + // - over the 1st && 2nd pass judge line AND + // - previously safe + // , + // which means ego can stop even if it is over the 1st pass judge line but + // - before default stopline OR + // - before the 2nd pass judge line OR + // - or previously unsafe + // . + // + // in order for ego to continue peeking or collision detection when occlusion is detected + // after ego passed the 1st pass judge line, it needs to be + // - before the default stopline OR + // - before the 2nd pass judge line OR + // - previously unsafe + // ========================================================================================== is_permanent_go_ = true; - return intersection::Result>::make_ok( - intersection::Indecisive{"over the pass judge line. no plan needed"}); - } - return intersection::Result>::make_err( - std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); + return { + is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, + safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -/* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel) - { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - // consider vehicle in ego-lane && in front of ego - const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive - const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); - - std::vector center_points; - for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); - // get the nearest centerpoint to object - std::vector dist_obj_center_points; - for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, - p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); - // find two center_points whose distances from `closest_centerpoint` cross stopping_distance - double acc_dist_prev = 0.0, acc_dist = 0.0; - auto p1 = center_points[obj_closest_centerpoint_idx]; - auto p2 = center_points[obj_closest_centerpoint_idx]; - for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } - } - // if stopping_distance >= center_points, stopping_point is center_points[end] - const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); - // linear interpolation - geometry_msgs::msg::Point stopping_point; - stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); - stopping_point.y = (p1.y * ratio + p2.y) / (1 + ratio); - stopping_point.z = (p1.z * ratio + p2.z) / (1 + ratio); - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, stopping_point); - stopping_point.x += lat_offset * std::cos(lane_yaw + M_PI / 2.0); - stopping_point.y += lat_offset * std::sin(lane_yaw + M_PI / 2.0); - - // calculate footprint of predicted stopping pose - autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; - predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; - predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); - const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); - debug_data_.predicted_obj_pose.position = stopping_point; - debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - - if (is_in_stuck_area) { - return true; - } - return false; - } -*/ - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8a34aabe8b918..752c21158ac95 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -19,6 +19,7 @@ #include "interpolated_path_info.hpp" #include "intersection_lanelets.hpp" #include "intersection_stoplines.hpp" +#include "object_manager.hpp" #include "result.hpp" #include @@ -45,32 +46,6 @@ namespace behavior_velocity_planner { -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -/** - * @struct - * @brief categorize TargetObjects - */ -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -154,6 +129,10 @@ class IntersectionModule : public SceneModuleInterface { double object_margin_to_path; } ignore_on_red_traffic_light; + struct AvoidCollisionByAcceleration + { + double object_time_margin_to_collision_point; + } avoid_collision_by_acceleration; } collision_detection; struct Occlusion @@ -204,36 +183,40 @@ class IntersectionModule : public SceneModuleInterface std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional first_pass_judge_wall_pose{std::nullopt}; + std::optional second_pass_judge_wall_pose{std::nullopt}; bool passed_first_pass_judge{false}; bool passed_second_pass_judge{false}; - std::optional second_pass_judge_wall_pose{std::nullopt}; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional first_attention_area{std::nullopt}; std::optional second_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; + + std::optional> + traffic_light_observation{std::nullopt}; }; using TimeDistanceArray = std::vector>; /** - * @struct * @brief categorize traffic light priority */ enum class TrafficPrioritizedLevel { @@ -246,6 +229,41 @@ class IntersectionModule : public SceneModuleInterface }; /** @} */ + /** + * @brief + */ + struct PassJudgeStatus + { + //! true if ego is over the 1st pass judge line + const bool is_over_1st_pass_judge; + + //! true if second_attention_lane exists and ego is over the 2nd pass judge line + const std::optional is_over_2nd_pass_judge; + + //! true only when ego passed 1st pass judge line safely for the first time + const bool safely_passed_1st_judge_line; + + //! true only when ego passed 2nd pass judge line safely for the first time + const bool safely_passed_2nd_judge_line; + }; + + /** + * @brief + */ + struct CollisionStatus + { + enum BlameType { + BLAME_AT_FIRST_PASS_JUDGE, + BLAME_AT_SECOND_PASS_JUDGE, + }; + const bool collision_detected; + const intersection::CollisionInterval::LanePosition collision_position; + const std::vector>> + too_late_detect_objects; + const std::vector>> + misjudge_objects; + }; + IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, @@ -325,6 +343,9 @@ class IntersectionModule : public SceneModuleInterface //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ std::nullopt}; + + //! save the time when ego observed green traffic light before entering the intersection + std::optional initial_green_light_observed_time_{std::nullopt}; /** @}*/ private: @@ -340,17 +361,19 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; - //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is - //! expected after these variables are non-null, then it is the fault of past perception failure - //! at these time. - std::optional safely_passed_1st_judge_line_time_{std::nullopt}; - std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + //! save the time and ego position when ego passed the 1st/2nd_pass_judge_line with safe + //! decision. If collision is expected after these variables are non-null, then it is the fault of + //! past perception failure at these time. + std::optional> + safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional> + safely_passed_2nd_judge_line_time_{std::nullopt}; /** @}*/ private: @@ -363,6 +386,23 @@ class IntersectionModule : public SceneModuleInterface */ //! debouncing for stable SAFE decision StateMachine collision_state_machine_; + + //! container for storing safety status of objects on the attention area + intersection::ObjectInfoManager object_info_manager_; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-variables [var] collision detection + * @{ + */ + //! save the last UNKNOWN traffic light information of this intersection(keep even if the info got + //! unavailable) + std::optional> tl_id_and_point_; + std::optional last_tl_valid_observation_{std::nullopt}; /** @} */ private: @@ -388,8 +428,6 @@ class IntersectionModule : public SceneModuleInterface StateMachine static_occlusion_timeout_state_machine_; /** @} */ - std::optional initial_green_light_observed_time_{std::nullopt}; - private: /** *********************************************************** @@ -464,13 +502,13 @@ class IntersectionModule : public SceneModuleInterface /** * @brief prepare basic data structure - * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @return return IntersectionStopLines if all data is valid, otherwise InternalError * @note if successful, it is ensure that intersection_lanelets_, * intersection_lanelets.first_conflicting_lane are not null * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( + intersection::Result prepareIntersectionData( const bool is_prioritized, PathWithLaneId * path); /** @@ -518,16 +556,6 @@ class IntersectionModule : public SceneModuleInterface const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); /** @} */ -private: - /** - * @defgroup utility [fn] utility member function - * @{ - */ - void stoppedAtPositionForDuration( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, - const double duration, StateMachine * state_machine); - /** @} */ - private: /** *********************************************************** @@ -545,15 +573,14 @@ class IntersectionModule : public SceneModuleInterface * @brief find TrafficPrioritizedLevel */ TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; - /** @} */ -private: /** - * @brief categorize target objects + * @brief update the valid traffic signal information if still available, otherwise keep last + * observation */ - TargetObjects generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const; + void updateTrafficSignalObservation(); + + /** @} */ private: /** @@ -598,14 +625,12 @@ class IntersectionModule : public SceneModuleInterface std::optional isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const; + const intersection::IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -621,27 +646,21 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check occlusion status * @attention this function has access to value() of occlusion_attention_divisions_, - * intersection_lanelets.first_attention_area() + * intersection_lanelets_ intersection_lanelets.first_attention_area() */ std::tuple< OcclusionType, bool /* module detection with margin */, bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects); + const intersection::InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); + OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info); /** @} */ private: @@ -654,15 +673,12 @@ class IntersectionModule : public SceneModuleInterface */ /** * @brief check if ego is already over the pass judge line - * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return + * @return if ego is over both 1st/2nd pass judge lines, return InternalError, else return * (is_over_1st_pass_judge, is_over_2nd_pass_judge) * @attention this function has access to value() of intersection_stoplines.default_stopline, * intersection_stoplines.occlusion_stopline */ - intersection::Result< - intersection::Indecisive, - std::pair> - isOverPassJudgeLinesStatus( + PassJudgeStatus isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -678,6 +694,25 @@ class IntersectionModule : public SceneModuleInterface bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + /** + * @brief find the objects on attention_area/intersection_area and update positional information + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerArea(); + + /** + * @brief find the collision Interval/CollisionKnowledge of registered objects + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time); + + void cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + /** * @brief check if there are any objects around the stoplines on the attention areas when ego * entered the intersection on green light @@ -687,44 +722,58 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_peeking_stopline */ std::optional isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects); + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines); + + /** + * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and + * blame past perception fault + */ + std::string generateDetectionBlameDiagnosis( + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; + + /** + * @brief generate the message explaining how much ego should accelerate to avoid future dangerous + * situation + */ + std::string generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const TimeDistanceArray & ego_time_distance_array, + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; /** - * @brief check collision + * @brief return if collision is detected and the collision position */ - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const intersection::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level); + CollisionStatus detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line); std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, const bool is_parked_vehicle) const; - void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); - /** * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position + * @attention this function has access to value() of + * intersection_stoplines.occlusion_peeking_stopline, + * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); /** @} */ - /* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel); - */ - mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 43bb68cf56f67..4496df77134e2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -18,42 +18,18 @@ #include // for toGeomPoly #include // for smoothPath #include +#include #include #include // for toPolygon2d #include -#include #include +#include +#include +#include #include -namespace -{ -tier4_autoware_utils::Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - namespace bg = boost::geometry; - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - tier4_autoware_utils::Polygon2d one_step_poly; - for (const auto & point : prev_poly.outer()) { - one_step_poly.outer().push_back(point); - } - for (const auto & point : next_poly.outer()) { - one_step_poly.outer().push_back(point); - } - - bg::correct(one_step_poly); - - tier4_autoware_utils::Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -79,38 +55,303 @@ bool IntersectionModule::isTargetCollisionVehicleType( return false; } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) +void IntersectionModule::updateObjectInfoManagerArea() { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - // If there are any vehicles on the attention area when ego entered the intersection on green - // light, do pseudo collision detection because the vehicles are very slow and no collisions may - // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = isGreenSolidOn(); - if (!is_green_solid_on) { - return std::nullopt; + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + + // ========================================================================================== + // entries that are not observed in this iteration need to be cleared + // + // NOTE: old_map is not referenced because internal data of object_info_manager is cleared + // ========================================================================================== + const auto old_map = object_info_manager_.getObjectsMap(); + object_info_manager_.clearObjects(); + + for (const auto & predicted_object : planner_data_->predicted_objects->objects) { + if (!isTargetCollisionVehicleType(predicted_object)) { + continue; + } + + // ========================================================================================== + // NOTE: is_parked_vehicle is used because sometimes slow vehicle direction is + // incorrect/reversed/flipped due to tracking. if is_parked_vehicle is true, object direction + // is not checked + // ========================================================================================== + const auto object_direction = + util::getObjectPoseWithVelocityDirection(predicted_object.kinematics); + const auto is_parked_vehicle = + std::fabs(predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); + if (belong_adjacent_lanelet_id) { + continue; + } + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + const bool in_intersection_area = [&]() { + if (!intersection_area) { + return false; + } + return bg::within( + tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + }(); + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + if (!belong_attention_lanelet_id && !in_intersection_area) { + continue; + } else if (belong_attention_lanelet_id) { + const auto idx = belong_attention_lanelet_id.value(); + attention_lanelet = attention_lanelets.at(idx); + stopline = attention_lanelet_stoplines.at(idx); + } + + const auto object_it = old_map.find(predicted_object.object_id); + if (object_it != old_map.end()) { + auto object_info = object_it->second; + object_info_manager_.registerExistingObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle, object_info); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } else { + auto object_info = object_info_manager_.registerObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } } - const auto closest_idx = intersection_stoplines.closest_idx; - const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - if (!initial_green_light_observed_time_) { - const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path.points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); +} + +void IntersectionModule::updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, + const IntersectionModule::TimeDistanceArray & time_distance_array, + const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time) +{ + const auto & intersection_lanelets = intersection_lanelets_.value(); + + if (passed_1st_judge_line_first_time) { + object_info_manager_.setPassed1stPassJudgeLineFirstTime(clock_->now()); + } + if (passed_2nd_judge_line_first_time) { + object_info_manager_.setPassed2ndPassJudgeLineFirstTime(clock_->now()); + } + + const double passing_time = time_distance_array.back().first; + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = + lanelet::utils::getArcCoordinates(concat_lanelets, planner_data_->current_odometry->pose); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // ========================================================================================== + // dynamically change TTC margin according to traffic light color to gradually relax from green to + // red + // ========================================================================================== + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + + for (auto & object_info : object_info_manager_.attentionObjects()) { + const auto & predicted_object = object_info->predicted_object(); + bool safe_under_traffic_control = false; + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + object_info->can_stop_before_stopline( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)) { + safe_under_traffic_control = true; + } + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + object_info->can_stop_before_ego_lane( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration, + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path, + ego_lane)) { + safe_under_traffic_control = true; + } + + // ========================================================================================== + // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe + // CollisionKnowledge for most probable path + // ========================================================================================== + std::list sorted_predicted_paths; + for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { + sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); + } + sorted_predicted_paths.sort( + [](const auto path1, const auto path2) { return path1->confidence > path2->confidence; }); + + // ========================================================================================== + // if all of the predicted path is lower confidence/geometrically does not intersect with ego + // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either + // of them has value, not both + // ========================================================================================== + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; + + for (const auto & predicted_path_ptr : sorted_predicted_paths) { + auto predicted_path = *predicted_path_ptr; + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + continue; + } + cutPredictPathWithinDuration( + planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); + const auto object_passage_interval_opt = intersection::findPassageInterval( + predicted_path, predicted_object.shape, ego_poly, + intersection_lanelets.first_attention_lane(), + intersection_lanelets.second_attention_lane()); + if (!object_passage_interval_opt) { + // there is no chance of geometric collision for the entire prediction horizon + continue; + } + const auto & object_passage_interval = object_passage_interval_opt.value(); + const auto [object_enter_time, object_exit_time] = object_passage_interval.interval_time; + const auto ego_start_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_start_itr == time_distance_array.end()) { + // ========================================================================================== + // this is the case where at time "object_enter_time - collision_start_margin_time", ego is + // arriving at the exit of the intersection, which means even if we assume that the object + // accelerates and the first collision happens faster by the TTC margin, ego will be already + // arriving at the exist of the intersection. + // ========================================================================================== + continue; + } + auto ego_end_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_end_itr == time_distance_array.end()) { + ego_end_itr = time_distance_array.end() - 1; + } + const double ego_start_arc_length = std::max( + 0.0, closest_arc_coords.length + ego_start_itr->second - + planner_data_->vehicle_info_.rear_overhang_m); + const double ego_end_arc_length = std::min( + closest_arc_coords.length + ego_end_itr->second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + const auto trimmed_ego_polygon = lanelet::utils::getPolygonFromArcLength( + concat_lanelets, ego_start_arc_length, ego_end_arc_length); + if (trimmed_ego_polygon.empty()) { + continue; + } + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + + const auto & object_path = object_passage_interval.path; + const auto [begin, end] = object_passage_interval.interval_position; + bool collision_detected = false; + for (auto i = begin; i <= end; ++i) { + if (bg::intersects( + polygon, + tier4_autoware_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + collision_detected = true; + break; + } + } + if (collision_detected) { + // if judged as UNSAFE, return + safe_interval = std::nullopt; + unsafe_interval = object_passage_interval; + break; + } + if (!safe_interval) { + // ========================================================================================== + // save the safe_decision_knowledge for the most probable path. this value is nullified if + // judged UNSAFE during the iteration + // ========================================================================================== + safe_interval = object_passage_interval; + } + } + object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); + if (passed_1st_judge_line_first_time) { + object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + if (passed_2nd_judge_line_first_time) { + object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + } +} + +void IntersectionModule::cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * path) const +{ + const rclcpp::Time current_time = clock_->now(); + const auto original_path = path->path; + path->path.clear(); + + for (size_t k = 0; k < original_path.size(); ++k) { // each path points + const auto & predicted_pose = original_path.at(k); + const auto predicted_time = + rclcpp::Time(object_stamp) + rclcpp::Duration(path->time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + path->path.push_back(predicted_pose); } } +} + +std::optional +IntersectionModule::isGreenPseudoCollisionStatus( + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines) +{ + // ========================================================================================== + // if there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because collision is likely to happen. + // ========================================================================================== if (initial_green_light_observed_time_) { const auto now = clock_->now(); const bool still_wait = @@ -119,22 +360,344 @@ IntersectionModule::isGreenPseudoCollisionStatus( if (!still_wait) { return std::nullopt; } + const auto & attention_objects = object_info_manager_.attentionObjects(); const bool exist_close_vehicles = std::any_of( - target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), - [&](const auto & object) { - return object.dist_to_stopline.has_value() && - object.dist_to_stopline.value() < - planner_param_.collision_detection.yield_on_green_traffic_light - .object_dist_to_stopline; + attention_objects.begin(), attention_objects.end(), [&](const auto & object_info) { + return object_info->before_stopline_by( + planner_param_.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline); }); if (exist_close_vehicles) { + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } return std::nullopt; } +std::string IntersectionModule::generateDetectionBlameDiagnosis( + const std::vector>> & + too_late_detect_objects, + const std::vector>> & + misjudge_objects) const +{ + std::string diag; + if (!safely_passed_1st_judge_line_time_) { + return diag; + } + const auto [passed_1st_judge_line_time, passed_1st_judge_line_pose] = + safely_passed_1st_judge_line_time_.value(); + const auto passed_1st_judge_line_time_double = + static_cast(passed_1st_judge_line_time.nanoseconds()) / 1e+9; + + const auto now = clock_->now(); + const auto now_double = static_cast(now.nanoseconds()) / 1e+9; + + // CAVEAT: format library causes runtime fault if the # of placeholders is more than the # of + // vargs + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval()) { + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_1st_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 1st pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the first attention lanelet of type " + "{5}.\n", + object_info->uuid_str, // 0 + passed_1st_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 1st pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + ); + } + } + if ( + safely_passed_2nd_judge_line_time_ && + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval()) { + const auto [passed_2nd_judge_line_time, passed_2nd_judge_line_pose] = + safely_passed_2nd_judge_line_time_.value(); + const auto passed_2nd_judge_line_time_double = + static_cast(passed_2nd_judge_line_time.nanoseconds()) / 1e+9; + + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_2nd_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 2nd pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the lanelet of type {5}.\n", + object_info->uuid_str, // 0 + passed_2nd_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 2nd pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + ); + } + } + } + for (const auto & [blame_type, object_info] : misjudge_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_1st_pass_judge_line_passage()) { + const auto & decision_at_1st_pass_judge_line = + object_info->decision_at_1st_pass_judge_line_passage().value(); + const auto decision_at_1st_pass_judge_line_time = + static_cast(decision_at_1st_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 1st pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the first attention lanelet of type {7} with the estimated " + "current velocity {8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_1st_pass_judge_line.safe_type), // 1 + decision_at_1st_pass_judge_line_time, // 2 + decision_at_1st_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + if ( + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_2nd_pass_judge_line_passage()) { + const auto & decision_at_2nd_pass_judge_line = + object_info->decision_at_2nd_pass_judge_line_passage().value(); + const auto decision_at_2nd_pass_judge_line_time = + static_cast(decision_at_2nd_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 2nd pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the lanelet of type {7} with the estimated current velocity " + "{8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_2nd_pass_judge_line.safe_type), // 1 + decision_at_2nd_pass_judge_line_time, // 2 + decision_at_2nd_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + } + return diag; +} + +std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const IntersectionModule::TimeDistanceArray & ego_time_distance_array, + const std::vector>> & + too_late_detect_objects, + [[maybe_unused]] const std::vector>> & + misjudge_objects) const +{ + static constexpr double min_vel = 1e-2; + std::string diag; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if (!object_info->unsafe_interval()) { + continue; + } + // object side + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const auto [begin, end] = unsafe_interval.interval_position; + const auto &p1 = unsafe_interval.path.at(begin).position, + p2 = unsafe_interval.path.at(end).position; + const auto collision_pos = + tier4_autoware_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto object_dist_to_margin_point = + tier4_autoware_utils::calcDistance2d( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, + collision_pos) - + planner_param_.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point * + object_info->observed_velocity(); + if (object_dist_to_margin_point <= 0.0) { + continue; + } + const auto object_eta_to_margin_point = + object_dist_to_margin_point / std::max(min_vel, object_info->observed_velocity()); + // ego side + const double ego_dist_to_collision_pos = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); + const auto ego_eta_to_collision_pos_it = std::lower_bound( + ego_time_distance_array.begin(), ego_time_distance_array.end(), ego_dist_to_collision_pos, + [](const auto & a, const double b) { return a.second < b; }); + if (ego_eta_to_collision_pos_it == ego_time_distance_array.end()) { + continue; + } + const double ego_eta_to_collision_pos = ego_eta_to_collision_pos_it->first; + if (ego_eta_to_collision_pos < object_eta_to_margin_point) { + // this case, ego will pass the collision point before this object get close to the collision + // point within margin just by keeping current plan, so no need to accelerate + continue; + } + diag += fmt::format( + "the object is expected to approach the collision point by the TTC margin in {0} seconds, " + "while ego will arrive there in {1} seconds, so ego needs to accelerate from current " + "velocity {2}[m/s] to {3}[m/s]\n", + object_eta_to_margin_point, // 0 + ego_eta_to_collision_pos, // 1 + ego_vel, // 2 + ego_dist_to_collision_pos / object_eta_to_margin_point // 3 + ); + } + return diag; +} + +IntersectionModule::CollisionStatus IntersectionModule::detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) +{ + // ========================================================================================== + // if collision is detected for multiple objects, we prioritize collision on the first + // attention lanelet + // ========================================================================================== + bool collision_at_first_lane = false; + bool collision_at_non_first_lane = false; + + // ========================================================================================== + // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // + // misjudge_objects are those that were once judged as safe when ego passed the pass judge line + // + // too_late_detect objects are those that (1) were not detected when ego passed the pass judge + // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are + // expected to have been detected in the prior iteration because ego could have judged as UNSAFE + // in the prior iteration + // ========================================================================================== + std::vector>> + misjudge_objects; + std::vector>> + too_late_detect_objects; + for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (object_info->is_safe_under_traffic_control()) { + debug_data_.safe_under_traffic_control_targets.objects.push_back( + object_info->predicted_object()); + continue; + } + if (!object_info->is_unsafe()) { + continue; + } + const auto & unsafe_info = object_info->is_unsafe().value(); + setObjectsOfInterestData( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose, + object_info->predicted_object().shape, ColorName::RED); + // ========================================================================================== + // if ego is over the pass judge lines, then the visualization as "too_late_objects" or + // "misjudge_objects" is more important than that for "unsafe" + // + // NOTE: consider a vehicle which was not detected at 1st_pass_judge_passage, and now collision + // detected on the 1st lane, which is "too_late" for 1st lane passage, but once it decelerated + // or yielded, so it turned safe, and ego passed the 2nd pass judge line, but at the same it + // accelerated again, which is "misjudge" for 2nd lane passage. In this case this vehicle is + // visualized as "misjudge" + // ========================================================================================== + auto * debug_container = &debug_data_.unsafe_targets.objects; + if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + collision_at_first_lane = true; + } else { + collision_at_non_first_lane = true; + } + if ( + is_over_1st_pass_judge_line && + unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + const auto & decision_at_1st_pass_judge_opt = + object_info->decision_at_1st_pass_judge_line_passage(); + if (!decision_at_1st_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); + if ( + decision_at_1st_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + if (is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value()) { + const auto & decision_at_2nd_pass_judge_opt = + object_info->decision_at_2nd_pass_judge_line_passage(); + if (!decision_at_2nd_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); + if ( + decision_at_2nd_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + debug_container->emplace_back(object_info->predicted_object()); + } + if (collision_at_first_lane) { + return { + true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + } else if (collision_at_non_first_lane) { + return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + } + return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; +} + +/* bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, const intersection::PathLanelets & path_lanelets, const size_t closest_idx, @@ -350,7 +913,7 @@ bool IntersectionModule::checkCollision( } } object_ttc_time_array.layout.dim.at(0).size++; - const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & pos = object..position; const auto & shape = object.shape; object_ttc_time_array.data.insert( object_ttc_time_array.data.end(), @@ -383,6 +946,7 @@ bool IntersectionModule::checkCollision( return collision_detected; } +*/ std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -422,32 +986,9 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( return std::nullopt; } -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) -{ - const rclcpp::Time current_time = clock_->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { const double intersection_velocity = @@ -460,8 +1001,42 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; const double current_velocity = planner_data_->current_velocity->twist.linear.x; - int assigned_lane_found = false; + // ========================================================================================== + // if ego is waiting for collision detection, the entry time into the intersection + // is a bit delayed for the chattering hold, so we need to "shift" the TimeDistanceArray by + // this delay + // ========================================================================================== + const bool is_go_out = (activated_ && occlusion_activated_); + const double time_delay = (is_go_out || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); + // ========================================================================================== + // to account for the stopline generated by upstream behavior_velocity modules (like walkway, + // crosswalk), if use_upstream flag is true, the raw velocity of path points after + // last_intersection_stopline_candidate_idx is used, which maybe almost-zero. at those almost-zero + // velocity path points, ego future profile is almost "fixed" there. + // + // last_intersection_stopline_candidate_idx must be carefully decided especially when ego + // velocity is almost zero, because if last_intersection_stopline_candidate_idx is at the + // closest_idx for example, ego is almost "fixed" at current position for the entire + // spatiotemporal profile, which is judged as SAFE because that profile does not collide + // with the predicted paths of objects. + // + // if second_attention_lane exists, second_attention_stopline_idx is used. if not, + // max(occlusion_stopline_idx, first_attention_stopline_idx) is used because + // occlusion_stopline_idx varies depending on the peeking offset parameter + // ========================================================================================== + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() + : std::max(occlusion_stopline_idx, first_attention_stopline_idx); + + int assigned_lane_found = false; // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 353826070eff7..5d82e328f71f7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -34,25 +34,34 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - const auto first_attention_area = intersection_lanelets.first_attention_area().value(); - const bool is_amber_or_red = + // ========================================================================================== + // for the convenience of Psim user, this module ignores occlusion if there has not been any + // information published for the associated traffic light even if occlusion.enable is true, + // and only runs collision checking on that intersection lane. + // + // this is because Psim-users/scenario-files do not set traffic light information perfectly + // most of the times, and they just set bare minimum traffic information only for traffic lights + // they are interested in or want to test. + // + // no_tl_info_ever variable is defined for that purpose. if there has been any + // information published for the associated traffic light in the real world through perception/V2I + // or in the simulation, then it should be kept in last_tl_valid_observation_ and this variable + // becomes false + // ========================================================================================== + const bool no_tl_info_ever = (has_traffic_light_ && !last_tl_valid_observation_.has_value()); + const bool is_amber_or_red_or_no_tl_info_ever = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) || no_tl_info_ever; // check occlusion on detection lane - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); auto occlusion_status = - (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? detectOcclusion( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) + (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && + !is_amber_or_red_or_no_tl_info_ever) + ? detectOcclusion(interpolated_path_info) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -76,13 +85,14 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & attention_areas = intersection_lanelets.occlusion_attention_area(); + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + const auto & lane_divisions = occlusion_attention_divisions_.value(); + const auto & occ_grid = *planner_data_->occupancy_grid; const auto & current_pose = planner_data_->current_odometry->pose; const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; @@ -208,13 +218,15 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + const auto & blocking_attention_objects = object_info_manager_.parkedObjects(); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + debug_data_.parked_targets.objects.push_back( + blocking_attention_object_info->predicted_object()); } std::vector> blocking_polygons; - for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + const Polygon2d obj_poly = + tier4_autoware_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -390,14 +402,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( LineString2d ego_occlusion_line; ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); - for (const auto & attention_object : target_objects.all_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - for (const auto & attention_object : target_objects.intersection_area_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + for (const auto & attention_object_info : object_info_manager_.allObjects()) { + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_line)) { return OcclusionType::DYNAMICALLY_OCCLUDED; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 746e615d8c6b0..5f62d10e60387 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -161,11 +161,13 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -intersection::Result +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + +Result IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) { - using intersection::Result; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); @@ -177,26 +179,32 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - return Result::make_err( - intersection::Indecisive{"splineInterpolate failed"}); + return make_err( + "splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return Result::make_err( - intersection::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}); + return make_err( + "Path has no interval on intersection lane " + std::to_string(lane_id_)); } - // cache intersection lane information because it is invariant if (!intersection_lanelets_) { intersection_lanelets_ = generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); } auto & intersection_lanelets = intersection_lanelets_.value(); - - // at the very first time of regisTration of this module, the path may not be conflicting with the - // attention area, so update() is called to update the internal data as well as traffic light info + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); + debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // ========================================================================================== + // at the very first time of registration of this module, the path may not be conflicting with + // the attention area, so update() is called to update the internal data as well as traffic + // light info + // ========================================================================================== intersection_lanelets.update( is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); @@ -205,17 +213,17 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return Result::make_err( - intersection::Indecisive{"conflicting area is empty"}); + return make_err( + "conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes + // ========================================================================================== + // even if the attention area is null, stuck vehicle stop line needs to be generated from + // conflicting lanes, so dummy_first_attention_lane is used + // ========================================================================================== const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() ? intersection_lanelets.first_attention_lane().value() : first_conflicting_lane; @@ -224,8 +232,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return Result::make_err( - intersection::Indecisive{"failed to generate intersection_stoplines"}); + return make_err( + "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); const auto closest_idx = intersection_stoplines.closest_idx; @@ -239,8 +247,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return Result::make_err( - intersection::Indecisive{"failed to generate PathLanelets"}); + return make_err( + "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -250,8 +258,32 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - return Result::make_ok( - BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); + // ========================================================================================== + // update traffic light information + // updateTrafficSignalObservation() must be called at first to because other traffic signal + // fuctions use last_valid_observation_ + // ========================================================================================== + if (has_traffic_light_) { + updateTrafficSignalObservation(); + const bool is_green_solid_on = isGreenSolidOn(); + if (is_green_solid_on && !initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + } + + return make_ok( + interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( @@ -421,8 +453,10 @@ IntersectionModule::generateIntersectionStopLines( int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { + // ========================================================================================== // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. + // ========================================================================================== const auto stuck_stopline_idx_ip_opt = util::getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); if (!stuck_stopline_idx_ip_opt) { @@ -457,12 +491,13 @@ IntersectionModule::generateIntersectionStopLines( second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge line position on interpolated path. It is the same as first pass judge - // line if second_attention_lane is null - int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; - const auto second_pass_judge_line_ip = - second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) - : first_pass_judge_line_ip; + // (8) second pass judge line position on interpolated path. It is null if second_attention_lane + // is null + size_t second_pass_judge_line_ip = occlusion_wo_tl_pass_judge_line_ip; + bool second_pass_judge_line_valid = false; + if (second_attention_area_opt) { + second_pass_judge_line_valid = true; + } struct IntersectionStopLinesTemp { @@ -528,9 +563,11 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } + if (second_pass_judge_line_valid) { + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + } intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; - intersection_stoplines.second_pass_judge_line = - intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -556,7 +593,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets } // for low priority lane - // If ego_lane has right of way (i.e. is high priority), + // if ego_lane has right of way (i.e. is high priority), // ignore yieldLanelets (i.e. low priority lanes) lanelet::ConstLanelets yield_lanelets{}; const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 7f23bed3c36cd..389c73d651f1e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -262,21 +262,19 @@ bool IntersectionModule::checkStuckVehicleInIntersection( std::optional IntersectionModule::isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const + const intersection::IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { return motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; - const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK - const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); // this is OK - const auto first_attention_stopline_idx = - intersection_stoplines.first_attention_stopline.value(); // this is OK + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding()); + interpolated_path_info, intersection_lanelets.attention_non_preceding()); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -297,14 +295,13 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { @@ -358,19 +355,20 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( ::createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); } debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { + for (const auto & object_info : object_info_manager_.attentionObjects()) { + const auto & object = object_info->predicted_object(); const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); if (obj_v_norm > stuck_vehicle_vel_thr) { continue; } for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); if (is_in_lanelet) { - debug_data_.yield_stuck_targets.objects.push_back(object.object); + debug_data_.yield_stuck_targets.objects.push_back(object); return true; } } diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 8b0511c741b4e..67da3c7a759fe 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -126,9 +126,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR if (!first_conflicting_idx_opt) { return false; } + // ========================================================================================== + // first_conflicting_idx is calculated considering baselink2front already, so there is no need + // to subtract baselink2front/ds here + // ========================================================================================== const auto stopline_idx_ip = static_cast(std::max( 0, static_cast(first_conflicting_idx_opt.value()) - - static_cast(baselink2front / planner_param_.path_interpolation_ds))); + static_cast(planner_param_.stopline_margin / planner_param_.path_interpolation_ds))); const auto stopline_idx_opt = util::insertPointIndex( interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f103191b2dc6f..e37bb3ee88cc1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -34,7 +34,6 @@ namespace behavior_velocity_planner::util { /** - * @fn * @brief insert a new pose to the path and return its index * @return if insertion was successful return the inserted point index */ @@ -44,7 +43,6 @@ std::optional insertPointIndex( const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** - * @fn * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( @@ -52,7 +50,6 @@ bool hasLaneIds( const std::set & ids); /** - * @fn * @brief find the first contiguous interval of the path points that contains the specified lane ids * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is * found, returns the pair (start-1, end) @@ -61,7 +58,6 @@ std::optional> findLaneIdsInterval( const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @fn * @brief return the index of the first point which is inside the given polygon * @param[in] lane_interval the interval of the path points on the intersection * @param[in] search_forward flag for search direction @@ -72,7 +68,6 @@ std::optional getFirstPointInsidePolygon( const bool search_forward = true); /** - * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -84,7 +79,6 @@ bool isOverTargetIndex( const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** - * @fn * @brief check if ego is before the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -99,13 +93,11 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** - * @fn * @brief check if the given lane has related traffic light */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); /** - * @fn * @brief interpolate PathWithLaneId */ std::optional generateInterpolatedPath( @@ -117,7 +109,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** - * @fn * @brief this function sorts the set of lanelets topologically using topological sort and merges * the lanelets from each root to each end. each branch is merged and returned with the original * lanelets @@ -131,7 +122,6 @@ mergeLaneletsByTopologicalSort( const lanelet::routing::RoutingGraphPtr routing_graph_ptr); /** - * @fn * @brief find the index of the first point where vehicle footprint intersects with the given * polygon */ @@ -140,6 +130,10 @@ std::optional getFirstPointInsidePolygonByFootprint( const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); +/** + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygons + */ std::optional> getFirstPointInsidePolygonsByFootprint( diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 47e643241740d..f38768ce26da4 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -406,7 +406,7 @@ bool generatePossibleCollisionsFromGridMap( const auto pc = generateOneNotableCollisionFromOcclusionSpot( grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, debug_data); - if (pc) continue; + if (pc == std::nullopt) continue; const double lateral_distance = std::abs(pc.value().arc_lane_dist_at_collision.distance); if (lateral_distance > distance_lower_bound) continue; distance_lower_bound = lateral_distance; diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 4bb0d8a2883dd..9792aa2bdd60b 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -8,6 +8,7 @@ Fumiya Watanabe Takamasa Horibe Makoto Kurihara + Satoshi Ota Apache License 2.0 Takamasa Horibe diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp index 99c01dcf66960..fb49a7fa446be 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -178,7 +178,7 @@ std::optional updateFrontPointForFix( const double lon_offset_to_prev_front = motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { - RCLCPP_WARN( + RCLCPP_DEBUG( rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; @@ -189,7 +189,7 @@ std::optional updateFrontPointForFix( // check if deviation is not too large constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { - RCLCPP_WARN( + RCLCPP_DEBUG( rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 532761b1f0cb7..8016107d96911 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -108,6 +108,20 @@ The obstacles meeting the following condition are determined as obstacles for cr | `behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold` | double | maximum obstacle velocity for cruise obstacle outside the trajectory | | `behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold` | double | maximum overlap time of the collision between the ego and obstacle | +##### Yield for vehicles that might cut in into the ego's lane + +It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane. + +The obstacles meeting the following condition are determined as obstacles for yielding (cruising). + +- The object type is for cruising according to `common.cruise_obstacle_type.*` and it is moving with a speed greater than `behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold`. +- The object is not crossing the ego's trajectory (\*1). +- There is another object of type `common.cruise_obstacle_type.*` stopped in front of the moving obstacle. +- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `behavior_determination.cruise.yield.max_lat_dist_between_obstacles` +- Both obstacles, moving and stopped, are within `behavior_determination.cruise.yield.lat_distance_threshold` and `behavior_determination.cruise.yield.lat_distance_threshold` + `behavior_determination.cruise.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively. + +If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle. + #### Determine stop vehicles Among obstacles which are not for cruising, the obstacles meeting the following condition are determined as obstacles for stopping. diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index aad4bd7e17757..22245e060b2ef 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -95,7 +95,12 @@ obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego - + yield: + enable_yield: false + lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_obstacles_collision_time: 10.0 # how far the blocking obstacle + stopped_obstacle_velocity_threshold: 0.5 slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 21cd980373df2..92c3a850b3989 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -55,7 +55,8 @@ struct Obstacle { Obstacle( const rclcpp::Time & arg_stamp, const PredictedObject & object, - const geometry_msgs::msg::Pose & arg_pose) + const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance, + const double lat_dist_from_obstacle_to_traj) : stamp(arg_stamp), pose(arg_pose), orientation_reliable(true), @@ -63,7 +64,9 @@ struct Obstacle twist_reliable(true), classification(object.classification.at(0)), uuid(tier4_autoware_utils::toHexString(object.object_id)), - shape(object.shape) + shape(object.shape), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) { predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -82,6 +85,8 @@ struct Obstacle std::string uuid; Shape shape; std::vector predicted_paths; + double ego_to_obstacle_distance; + double lat_dist_from_obstacle_to_traj; }; struct TargetObstacleInterface diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 4a3654d48afe1..a8328d536f13c 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -70,6 +70,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::optional createCollisionPointForStopObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle) const; + std::optional createYieldCruiseObstacle( + const Obstacle & obstacle, const std::vector & traj_points); + std::optional> findYieldCruiseObstacles( + const std::vector & obstacles, const std::vector & traj_points); std::optional createCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lat_dist); @@ -196,8 +200,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node int successive_num_to_entry_slow_down_condition; int successive_num_to_exit_slow_down_condition; // consideration for the current ego pose - bool enable_to_consider_current_pose{false}; double time_to_convergence{1.5}; + bool enable_to_consider_current_pose{false}; + // yield related parameters + bool enable_yield{false}; + double yield_lat_distance_threshold; + double max_lat_dist_between_obstacles; + double stopped_obstacle_velocity_threshold; + double max_obstacles_collision_time; }; BehaviorDeterminationParam behavior_determination_param_; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ee1d97f2c6c01..e4e6cdf9e0873 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -54,20 +54,15 @@ std::optional getObstacleFromUuid( return *itr; } -bool isFrontObstacle( +std::optional calcDistanceToFrontVehicle( const std::vector & traj_points, const size_t ego_idx, const geometry_msgs::msg::Point & obstacle_pos) { const size_t obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle_pos); - - const double ego_to_obstacle_distance = + const auto ego_to_obstacle_distance = motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx); - - if (ego_to_obstacle_distance < 0) { - return false; - } - - return true; + if (ego_to_obstacle_distance < 0.0) return std::nullopt; + return ego_to_obstacle_distance; } PredictedPath resampleHighestConfidencePredictedPath( @@ -254,6 +249,15 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara node.declare_parameter("behavior_determination.stop.max_lat_margin"); max_lat_margin_for_cruise = node.declare_parameter("behavior_determination.cruise.max_lat_margin"); + enable_yield = node.declare_parameter("behavior_determination.cruise.yield.enable_yield"); + yield_lat_distance_threshold = + node.declare_parameter("behavior_determination.cruise.yield.lat_distance_threshold"); + max_lat_dist_between_obstacles = node.declare_parameter( + "behavior_determination.cruise.yield.max_lat_dist_between_obstacles"); + stopped_obstacle_velocity_threshold = node.declare_parameter( + "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold"); + max_obstacles_collision_time = node.declare_parameter( + "behavior_determination.cruise.yield.max_obstacles_collision_time"); max_lat_margin_for_slow_down = node.declare_parameter("behavior_determination.slow_down.max_lat_margin"); lat_hysteresis_margin_for_slow_down = @@ -309,6 +313,20 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); tier4_autoware_utils::updateParam( parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.cruise.yield.lat_distance_threshold", + yield_lat_distance_threshold); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles", + max_lat_dist_between_obstacles); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold", + stopped_obstacle_velocity_threshold); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time", + max_obstacles_collision_time); tier4_autoware_utils::updateParam( parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down); tier4_autoware_utils::updateParam( @@ -493,7 +511,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (3) not too far from trajectory const auto target_obstacles = convertToObstacles(traj_points); - // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. + // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles); @@ -590,6 +608,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( stop_watch_.tic(__func__); const auto obj_stamp = rclcpp::Time(objects_ptr_->header.stamp); + const auto & p = behavior_determination_param_; std::vector target_obstacles; for (const auto & predicted_object : objects_ptr_->objects) { @@ -611,9 +630,9 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( // 2. Check if the obstacle is in front of the ego. const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, ego_odom_ptr_->pose.pose); - const bool is_front_obstacle = - isFrontObstacle(traj_points, ego_idx, current_obstacle_pose.pose.position); - if (!is_front_obstacle) { + const auto ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); + if (!ego_to_obstacle_distance) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", object_id.c_str()); @@ -621,14 +640,15 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } // 3. Check if rough lateral distance is smaller than the threshold + const double lat_dist_from_obstacle_to_traj = + motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); + const double min_lat_dist_to_traj_poly = [&]() { - const double lat_dist_from_obstacle_to_traj = - motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - obstacle_max_length; }(); - const auto & p = behavior_determination_param_; + const double max_lat_margin = std::max( std::max(p.max_lat_margin_for_stop, p.max_lat_margin_for_cruise), p.max_lat_margin_for_slow_down); @@ -639,7 +659,9 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( continue; } - const auto target_obstacle = Obstacle(obj_stamp, predicted_object, current_obstacle_pose.pose); + const auto target_obstacle = Obstacle( + obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), + lat_dist_from_obstacle_to_traj); target_obstacles.push_back(target_obstacle); } @@ -741,6 +763,23 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( continue; } } + const auto & p = behavior_determination_param_; + if (p.enable_yield) { + const auto yield_obstacles = findYieldCruiseObstacles(obstacles, decimated_traj_points); + if (yield_obstacles) { + for (const auto & y : yield_obstacles.value()) { + // Check if there is no member with the same UUID in cruise_obstacles + auto it = std::find_if( + cruise_obstacles.begin(), cruise_obstacles.end(), + [&y](const auto & c) { return y.uuid == c.uuid; }); + + // If no matching UUID found, insert yield obstacle into cruise_obstacles + if (it == cruise_obstacles.end()) { + cruise_obstacles.push_back(y); + } + } + } + } slow_down_condition_counter_.removeCounterUnlessUpdated(); // Check target obstacles' consistency @@ -829,6 +868,137 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( tangent_vel, normal_vel, *collision_points}; } +std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( + const Obstacle & obstacle, const std::vector & traj_points) +{ + if (traj_points.empty()) return std::nullopt; + // check label + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + + if (!isOutsideCruiseObstacle(obstacle.classification.label)) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore yield obstacle (%s) since its type is not designated.", object_id.c_str()); + return std::nullopt; + } + + if ( + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) < + p.outside_obstacle_min_velocity_threshold) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore yield obstacle (%s) since the obstacle velocity is low.", object_id.c_str()); + return std::nullopt; + } + + if (isObstacleCrossing(traj_points, obstacle)) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore yield obstacle (%s) since it's crossing the ego's trajectory..", + object_id.c_str()); + return std::nullopt; + } + + const auto collision_points = [&]() -> std::optional> { + const auto obstacle_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose); + if (!obstacle_idx) return std::nullopt; + const auto collision_traj_point = traj_points.at(obstacle_idx.value()); + const auto object_time = now() + traj_points.at(obstacle_idx.value()).time_from_start; + + PointWithStamp collision_traj_point_with_stamp; + collision_traj_point_with_stamp.stamp = object_time; + collision_traj_point_with_stamp.point.x = collision_traj_point.pose.position.x; + collision_traj_point_with_stamp.point.y = collision_traj_point.pose.position.y; + collision_traj_point_with_stamp.point.z = collision_traj_point.pose.position.z; + std::vector collision_points_vector{collision_traj_point_with_stamp}; + return collision_points_vector; + }(); + + if (!collision_points) return std::nullopt; + const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); + // check if obstacle is driving on the opposite direction + if (tangent_vel < 0.0) return std::nullopt; + return CruiseObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, + tangent_vel, normal_vel, collision_points.value()}; +} + +std::optional> ObstacleCruisePlannerNode::findYieldCruiseObstacles( + const std::vector & obstacles, const std::vector & traj_points) +{ + if (obstacles.empty() || traj_points.empty()) return std::nullopt; + const auto & p = behavior_determination_param_; + + std::vector stopped_obstacles; + std::vector moving_obstacles; + + std::for_each( + obstacles.begin(), obstacles.end(), + [&stopped_obstacles, &moving_obstacles, &p](const auto & o) { + const bool is_moving = + std::hypot(o.twist.linear.x, o.twist.linear.y) > p.stopped_obstacle_velocity_threshold; + if (is_moving) { + const bool is_within_lat_dist_threshold = + o.lat_dist_from_obstacle_to_traj < p.yield_lat_distance_threshold; + if (is_within_lat_dist_threshold) moving_obstacles.push_back(o); + return; + } + // lat threshold is larger for stopped obstacles + const bool is_within_lat_dist_threshold = + o.lat_dist_from_obstacle_to_traj < + p.yield_lat_distance_threshold + p.max_lat_dist_between_obstacles; + if (is_within_lat_dist_threshold) stopped_obstacles.push_back(o); + return; + }); + + if (stopped_obstacles.empty() || moving_obstacles.empty()) return std::nullopt; + + std::sort( + stopped_obstacles.begin(), stopped_obstacles.end(), [](const auto & o1, const auto & o2) { + return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance; + }); + + std::sort(moving_obstacles.begin(), moving_obstacles.end(), [](const auto & o1, const auto & o2) { + return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance; + }); + + std::vector yield_obstacles; + for (const auto & moving_obstacle : moving_obstacles) { + for (const auto & stopped_obstacle : stopped_obstacles) { + const bool is_moving_obs_behind_of_stopped_obs = + moving_obstacle.ego_to_obstacle_distance < stopped_obstacle.ego_to_obstacle_distance; + const bool is_moving_obs_ahead_of_ego_front = + moving_obstacle.ego_to_obstacle_distance > vehicle_info_.vehicle_length_m; + + if (!is_moving_obs_ahead_of_ego_front || !is_moving_obs_behind_of_stopped_obs) continue; + + const double lateral_distance_between_obstacles = std::abs( + moving_obstacle.lat_dist_from_obstacle_to_traj - + stopped_obstacle.lat_dist_from_obstacle_to_traj); + + const double longitudinal_distance_between_obstacles = std::abs( + moving_obstacle.ego_to_obstacle_distance - stopped_obstacle.ego_to_obstacle_distance); + + const double moving_obstacle_speed = + std::hypot(moving_obstacle.twist.linear.x, moving_obstacle.twist.linear.y); + + const bool are_obstacles_aligned = + lateral_distance_between_obstacles < p.max_lat_dist_between_obstacles; + const bool obstacles_collide_within_threshold_time = + longitudinal_distance_between_obstacles / moving_obstacle_speed < + p.max_obstacles_collision_time; + if (are_obstacles_aligned && obstacles_collide_within_threshold_time) { + const auto yield_obstacle = createYieldCruiseObstacle(moving_obstacle, traj_points); + if (yield_obstacle) { + yield_obstacles.push_back(*yield_obstacle); + } + } + } + } + if (yield_obstacles.empty()) return std::nullopt; + return yield_obstacles; +} + std::optional> ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 050c4af55c9bf..3bdfd67a7bec4 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -1548,6 +1548,8 @@ void ObstacleStopPlannerNode::filterObstacles( const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, PredictedObjects & filtered_objects) { + if (traj.size() < 2) return; + filtered_objects.header = input_objects.header; for (auto & object : input_objects.objects) { diff --git a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp index 724c1a966781f..067f5d2c57cc4 100644 --- a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp +++ b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp @@ -136,7 +136,7 @@ std::optional updateFrontPointForFix( const double lon_offset_to_prev_front = motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { - RCLCPP_WARN( + RCLCPP_DEBUG( rclcpp::get_logger("path_smoother.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; @@ -147,7 +147,7 @@ std::optional updateFrontPointForFix( // check if deviation is not too large constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { - RCLCPP_WARN( + RCLCPP_DEBUG( rclcpp::get_logger("path_smoother.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } diff --git a/planning/rtc_interface/package.xml b/planning/rtc_interface/package.xml index 78e0f39292075..753e4df13908e 100644 --- a/planning/rtc_interface/package.xml +++ b/planning/rtc_interface/package.xml @@ -7,6 +7,7 @@ Fumiya Watanabe Taiki Tanaka Kyoichi Sugahara + Satoshi Ota Apache License 2.0 diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp index c98e6d97969d4..7d60214a52722 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp +++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp @@ -18,6 +18,7 @@ #include "frenet_planner/structures.hpp" #include "sampler_common/structures.hpp" #include "sampler_common/transform/spline_transform.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index a4ff1db77e8a1..5b395883dd65d 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -22,6 +22,11 @@ #include #include +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include + +#include + #include #include #include @@ -150,6 +155,7 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P path.yaws.reserve(path.frenet_points.size()); path.lengths.reserve(path.frenet_points.size()); path.curvatures.reserve(path.frenet_points.size()); + path.poses.reserve(path.frenet_points.size()); // Calculate cartesian positions for (const auto & fp : path.frenet_points) { path.points.push_back(reference.cartesian(fp)); @@ -161,10 +167,20 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P for (auto it = path.points.begin(); it != std::prev(path.points.end()); ++it) { const auto dx = std::next(it)->x() - it->x(); const auto dy = std::next(it)->y() - it->y(); - path.yaws.push_back(std::atan2(dy, dx)); + const auto yaw = std::atan2(dy, dx); + path.yaws.push_back(yaw); path.lengths.push_back(path.lengths.back() + std::hypot(dx, dy)); + + geometry_msgs::msg::Pose pose; + pose.position.x = it->x(); + pose.position.y = it->y(); + pose.position.z = 0.0; + pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + path.poses.push_back(pose); } path.yaws.push_back(path.yaws.back()); + path.poses.push_back(path.poses.back()); + // Calculate curvatures for (size_t i = 1; i < path.yaws.size(); ++i) { const auto dyaw = diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp index 475aae4aeea18..652167ad94e75 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp @@ -17,10 +17,14 @@ #include "sampler_common/structures.hpp" +#include namespace sampler_common::constraints { /// @brief Check if the path satisfies the hard constraints MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints); +bool has_collision(const MultiPoint2d & footprint, const MultiPolygon2d & obstacles); +bool satisfyMinMax(const std::vector & values, const double min, const double max); + } // namespace sampler_common::constraints #endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp index 1cb14fdf56198..05c7fe47d79ef 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp @@ -20,16 +20,20 @@ #include #include +#include + #include #include #include #include +#include #include #include #include #include #include +#include #include namespace sampler_common @@ -40,6 +44,9 @@ using tier4_autoware_utils::MultiPolygon2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +typedef std::pair BoxIndexPair; +typedef boost::geometry::index::rtree> Rtree; + /// @brief data about constraint check results of a given path struct ConstraintResults { @@ -79,6 +86,9 @@ struct Path std::vector curvatures{}; std::vector yaws{}; std::vector lengths{}; + std::vector poses{}; + + bool constraints_satisfied; ConstraintResults constraint_results{}; double cost{}; std::string tag{}; // string tag used for debugging @@ -92,6 +102,7 @@ struct Path curvatures.clear(); yaws.clear(); lengths.clear(); + poses.clear(); constraint_results.clear(); tag = ""; cost = 0.0; @@ -103,6 +114,7 @@ struct Path curvatures.reserve(size); yaws.reserve(size); lengths.reserve(size); + poses.reserve(size); } [[nodiscard]] Path extend(const Path & path) const @@ -126,6 +138,7 @@ struct Path ext(extended_path.points, points, path.points); ext(extended_path.curvatures, curvatures, path.curvatures); ext(extended_path.yaws, yaws, path.yaws); + ext(extended_path.poses, poses, path.poses); extended_path.lengths.insert(extended_path.lengths.end(), lengths.begin(), lengths.end()); const auto last_base_length = lengths.empty() ? 0.0 : lengths.back() + length_offset; for (size_t i = offset; i < path.lengths.size(); ++i) @@ -327,6 +340,7 @@ struct Constraints double lateral_deviation_weight; double length_weight; double curvature_weight; + std::vector weights; } soft{}; struct { @@ -339,6 +353,7 @@ struct Constraints MultiPolygon2d obstacle_polygons; MultiPolygon2d drivable_polygons; std::vector dynamic_obstacles; + Rtree rtree; }; struct ReusableTrajectory diff --git a/sensing/gnss_poser/schema/gnss_poser.schema.json b/sensing/gnss_poser/schema/gnss_poser.schema.json index 6d9bc716e121a..d6e104b7432f2 100644 --- a/sensing/gnss_poser/schema/gnss_poser.schema.json +++ b/sensing/gnss_poser/schema/gnss_poser.schema.json @@ -42,7 +42,6 @@ }, "required": [ "base_frame", - "gnss_frame", "gnss_base_frame", "map_frame", "use_gnss_ins_orientation", diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 7af82c1129aff..2df12c94b7d3b 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -70,11 +70,11 @@ In the future, with careful implementation for pose errors, the IMU bias estimat ### Parameters +Note that this node also uses `angular_velocity_offset_x`, `angular_velocity_offset_y`, `angular_velocity_offset_z` parameters from `imu_corrector.param.yaml`. + | Name | Type | Description | | ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | -| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | -| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | -| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | | `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | | `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] | +| `diagnostics_updater_interval_sec` | double | period of the diagnostics updater [sec] | | `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index e10329c920137..eac423f94fa78 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -2,4 +2,5 @@ ros__parameters: gyro_bias_threshold: 0.0015 # [rad/s] timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 50e4a702ec949..e99667ed1c4a6 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -30,6 +30,7 @@ GyroBiasEstimator::GyroBiasEstimator() angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + diagnostics_updater_interval_sec_(declare_parameter("diagnostics_updater_interval_sec")), straight_motion_ang_vel_upper_limit_( declare_parameter("straight_motion_ang_vel_upper_limit")), updater_(this), @@ -37,6 +38,8 @@ GyroBiasEstimator::GyroBiasEstimator() { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + // diagnostic_updater is designed to be updated at the same rate as the timer + updater_.setPeriod(diagnostics_updater_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -57,6 +60,18 @@ GyroBiasEstimator::GyroBiasEstimator() this->get_node_timers_interface()->add_timer(timer_, nullptr); transform_listener_ = std::make_shared(this); + + // initialize diagnostics_info_ + { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Not initialized"; + diagnostics_info_.gyro_bias_x_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_y_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_z_for_imu_corrector = std::nan(""); + diagnostics_info_.estimated_gyro_bias_x = std::nan(""); + diagnostics_info_.estimated_gyro_bias_y = std::nan(""); + diagnostics_info_.estimated_gyro_bias_z = std::nan(""); + } } void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) @@ -99,6 +114,7 @@ void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_pt void GyroBiasEstimator::timer_callback() { if (pose_buf_.empty()) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is empty)"; return; } @@ -112,6 +128,7 @@ void GyroBiasEstimator::timer_callback() const rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf.front().header.stamp); const rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf.back().header.stamp); if (t1_rclcpp_time <= t0_rclcpp_time) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is not in chronological order)"; return; } @@ -127,6 +144,7 @@ void GyroBiasEstimator::timer_callback() // Check gyro data size // Data size must be greater than or equal to 2 since the time difference will be taken later if (gyro_filtered.size() <= 1) { + diagnostics_info_.summary_message = "Skipped update (gyro_filtered size is less than 2)"; return; } @@ -140,6 +158,8 @@ void GyroBiasEstimator::timer_callback() const double yaw_vel = yaw_diff / time_diff; const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); if (!is_straight) { + diagnostics_info_.summary_message = + "Skipped update (yaw angular velocity is greater than straight_motion_ang_vel_upper_limit)"; return; } @@ -151,12 +171,45 @@ void GyroBiasEstimator::timer_callback() if (!tf_base2imu_ptr) { RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + + diagnostics_info_.summary_message = "Skipped update (tf between base and imu is not available)"; return; } + gyro_bias_ = transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); + validate_gyro_bias(); updater_.force_update(); + updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater +} + +void GyroBiasEstimator::validate_gyro_bias() +{ + // Calculate diagnostics key-values + diagnostics_info_.gyro_bias_x_for_imu_corrector = gyro_bias_.value().x; + diagnostics_info_.gyro_bias_y_for_imu_corrector = gyro_bias_.value().y; + diagnostics_info_.gyro_bias_z_for_imu_corrector = gyro_bias_.value().z; + diagnostics_info_.estimated_gyro_bias_x = gyro_bias_.value().x - angular_velocity_offset_x_; + diagnostics_info_.estimated_gyro_bias_y = gyro_bias_.value().y - angular_velocity_offset_y_; + diagnostics_info_.estimated_gyro_bias_z = gyro_bias_.value().z - angular_velocity_offset_z_; + + // Validation + const bool is_bias_small_enough = + std::abs(diagnostics_info_.estimated_gyro_bias_x) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_y) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_z) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Successfully updated"; + } else { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diagnostics_info_.summary_message = + "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in imu_corrector. " + "You may also use the output of gyro_bias_estimator."; + } } geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( @@ -172,36 +225,22 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { - if (gyro_bias_ == std::nullopt) { - stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); - } else { - stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); - stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); - stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); - - stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); - stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); - stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); - - // Validation - const bool is_bias_small_enough = - std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && - std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && - std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; - - // Update diagnostics - if (is_bias_small_enough) { - stat.add("gyro_bias", "OK"); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); - } else { - stat.add( - "gyro_bias", - "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " - "imu_corrector. You may also use the output of gyro_bias_estimator."); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); - } - } + auto f = [](const double & value) { + std::stringstream ss; + ss << std::fixed << std::setprecision(8) << value; + return ss.str(); + }; + + stat.summary(diagnostics_info_.level, diagnostics_info_.summary_message); + stat.add("gyro_bias_x_for_imu_corrector", f(diagnostics_info_.gyro_bias_x_for_imu_corrector)); + stat.add("gyro_bias_y_for_imu_corrector", f(diagnostics_info_.gyro_bias_y_for_imu_corrector)); + stat.add("gyro_bias_z_for_imu_corrector", f(diagnostics_info_.gyro_bias_z_for_imu_corrector)); + + stat.add("estimated_gyro_bias_x", f(diagnostics_info_.estimated_gyro_bias_x)); + stat.add("estimated_gyro_bias_y", f(diagnostics_info_.estimated_gyro_bias_y)); + stat.add("estimated_gyro_bias_z", f(diagnostics_info_.estimated_gyro_bias_z)); + + stat.add("gyro_bias_threshold", f(gyro_bias_threshold_)); } } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 821cba0b213ff..3592ff1f7d3b4 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -49,6 +49,7 @@ class GyroBiasEstimator : public rclcpp::Node void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr); void timer_callback(); + void validate_gyro_bias(); static geometry_msgs::msg::Vector3 transform_vector3( const geometry_msgs::msg::Vector3 & vec, @@ -68,6 +69,7 @@ class GyroBiasEstimator : public rclcpp::Node const double angular_velocity_offset_y_; const double angular_velocity_offset_z_; const double timer_callback_interval_sec_; + const double diagnostics_updater_interval_sec_; const double straight_motion_ang_vel_upper_limit_; diagnostic_updater::Updater updater_; @@ -80,6 +82,20 @@ class GyroBiasEstimator : public rclcpp::Node std::vector gyro_all_; std::vector pose_buf_; + + struct DiagnosticsInfo + { + unsigned char level; + std::string summary_message; + double gyro_bias_x_for_imu_corrector; + double gyro_bias_y_for_imu_corrector; + double gyro_bias_z_for_imu_corrector; + double estimated_gyro_bias_x; + double estimated_gyro_bias_y; + double estimated_gyro_bias_z; + }; + + DiagnosticsInfo diagnostics_info_; }; } // namespace imu_corrector diff --git a/sensing/pointcloud_preprocessor/README.md b/sensing/pointcloud_preprocessor/README.md index 91ef34f4cae72..5c6402efdf23d 100644 --- a/sensing/pointcloud_preprocessor/README.md +++ b/sensing/pointcloud_preprocessor/README.md @@ -56,7 +56,45 @@ 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 of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). +`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 + +In Autoware, point cloud data from each LiDAR sensor undergoes preprocessing in the sensing pipeline before being input +into the perception pipeline. The preprocessing stages are illustrated in the diagram below: + +![pointcloud_preprocess_pipeline.drawio.png](docs%2Fimage%2Fpointcloud_preprocess_pipeline.drawio.png) + +Each stage in the pipeline incurs a processing delay. Mostly, we've used `ros2 topic delay /topic_name` to measure +the time between the message header and the current time. This approach works well for small-sized messages. However, +when dealing with large point cloud messages, this method introduces an additional delay. This is primarily because +accessing these large point cloud messages externally impacts the pipeline's performance. + +Our sensing/perception nodes are designed to run within composable node containers, leveraging intra-process +communication. External subscriptions to these messages (like using ros2 topic delay or rviz2) impose extra delays and +can even slow down the pipeline by subscribing externally. Therefore, these measurements will not be accurate. + +To mitigate this issue, we've adopted a method where each node in the pipeline reports its pipeline latency time. +This approach ensures the integrity of intra-process communication and provides a more accurate measure of delays in the +pipeline. + +### Benchmarking The Pipeline + +The nodes within the pipeline report the pipeline latency time, indicating the duration from the sensor driver's pointcloud +output to the node's output. This data is crucial for assessing the pipeline's health and efficiency. + +When running Autoware, you can monitor the pipeline latency times for each node in the pipeline by subscribing to the +following ROS 2 topics: + +- `/sensing/lidar/LidarX/crop_box_filter_self/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/crop_box_filter_mirror/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/distortion_corrector/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/ring_outlier_filter/debug/pipeline_latency_ms` +- `/sensing/lidar/concatenate_data_synchronizer/debug/sensing/lidar/LidarX/pointcloud/pipeline_latency_ms` + +These topics provide the pipeline latency times, giving insights into the delays at various stages of the pipeline +from the sensor output of LidarX to each subsequent node. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png b/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png new file mode 100644 index 0000000000000..e690d0179afa6 Binary files /dev/null and b/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png differ diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index eedc8e6bcb573..42ca694235a00 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,12 +22,13 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ----------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | +| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index f655a9245ca6d..ab7d4e0012304 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -46,6 +46,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; + size_t max_points_num_per_ring_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -53,6 +54,21 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + bool isCluster( + const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) + { + if (walk_size > num_points_threshold_) return true; + + auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); + auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); + + const auto x = first_point->x - last_point->x; + const auto y = first_point->y - last_point->y; + const auto z = first_point->z - last_point->z; + + return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; + } + public: PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index e3090f34d6854..a65d14c0a1194 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -31,7 +31,6 @@ pcl_msgs pcl_ros point_cloud_msg_wrapper - range-v3 rclcpp rclcpp_components sensor_msgs diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index ff72e433e9602..f0b04bacf26dd 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -318,6 +318,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( for (const auto & e : cloud_stdmap_) { transformed_clouds[e.first] = nullptr; if (e.second != nullptr) { + if (e.second->data.size() == 0) { + continue; + } pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); } } @@ -332,6 +335,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( // Step2. Calculate compensation transform and concatenate with the oldest stamp for (const auto & e : cloud_stdmap_) { if (e.second != nullptr) { + if (e.second->data.size() == 0) { + continue; + } sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); transformPointCloud(e.second, transformed_cloud_ptr); @@ -380,6 +386,20 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() const auto & transformed_raw_points = PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + if (debug_publisher_) { + const auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); + } + } + } + // publish concatenated pointcloud if (concat_cloud_ptr) { auto output = std::make_unique(*concat_cloud_ptr); @@ -479,16 +499,16 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { std::lock_guard lock(mutex_); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); auto input = std::make_shared(*input_ptr); if (input->data.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - return; + } else { + // convert to XYZI pointcloud if pointcloud is not empty + convertToXYZICloud(input, xyzi_input_ptr); } - sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); - convertToXYZICloud(input, xyzi_input_ptr); - const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 20d1f5c8b3d6d..cfbeffee9982c 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -65,7 +65,7 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "crop_box_filter"); + debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -195,6 +195,14 @@ void CropBoxFilterComponent::faster_filter( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index dd38b85a2b56d..d1d91ed7ec439 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -128,6 +128,16 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms undistortPointCloud(tf2_base_link_to_sensor, *points_msg); + if (debug_publisher_) { + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - points_msg->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } + undistorted_points_pub_->publish(std::move(points_msg)); // add processing time for debug diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index e277b221a090d..d968b06a0dc61 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,18 +14,10 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" -#include "pointcloud_preprocessor/utility/utilities.hpp" - -#include -#include -#include - #include #include -#include #include - namespace pointcloud_preprocessor { RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) @@ -48,6 +40,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("object_length_threshold", 0.1)); num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); + max_points_num_per_ring_ = + static_cast(declare_parameter("max_points_num_per_ring", 4000)); } using std::placeholders::_1; @@ -67,252 +61,122 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - // The ring_outlier_filter specifies the expected input point cloud format, - // however, we want to verify the input is correct and make failures explicit. - auto getFieldOffsetSafely = [=]( - const std::string & field_name, - const pcl::PCLPointField::PointFieldTypes expected_type) -> size_t { - const auto field_index = pcl::getFieldIndex(*input, field_name); - if (field_index == -1) { - RCLCPP_ERROR(get_logger(), "Field %s not found in input point cloud", field_name.c_str()); - return -1UL; - } - - auto field = (*input).fields.at(field_index); - if (field.datatype != expected_type) { - RCLCPP_ERROR( - get_logger(), "Field %s has unexpected type %d (expected %d)", field_name.c_str(), - field.datatype, expected_type); - return -1UL; - } - - return static_cast(field.offset); - }; + output.point_step = sizeof(PointXYZI); + output.data.resize(output.point_step * input->width); + size_t output_size = 0; - // as per the specification of this node, these fields must be present in the input - const auto ring_offset = getFieldOffsetSafely("ring", pcl::PCLPointField::UINT16); - const auto azimuth_offset = getFieldOffsetSafely("azimuth", pcl::PCLPointField::FLOAT32); - const auto distance_offset = getFieldOffsetSafely("distance", pcl::PCLPointField::FLOAT32); - const auto intensity_offset = getFieldOffsetSafely("intensity", pcl::PCLPointField::FLOAT32); + const auto ring_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; + const auto azimuth_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; + const auto distance_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; + const auto intensity_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; + + std::vector> ring2indices; + ring2indices.reserve(max_rings_num_); + + for (uint16_t i = 0; i < max_rings_num_; i++) { + ring2indices.push_back(std::vector()); + ring2indices.back().reserve(max_points_num_per_ring_); + } - if ( - ring_offset == -1UL || azimuth_offset == -1UL || distance_offset == -1UL || - intensity_offset == -1UL) { - RCLCPP_ERROR(get_logger(), "One or more required fields are missing in input point cloud"); - return; + for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { + const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); + ring2indices[ring].push_back(data_idx); } - // The initial implementation of ring outlier filter looked like this: - // 1. Iterate over the input cloud and group point indices by ring - // 2. For each ring: - // 2.1. iterate over the ring points, and group points belonging to the same "walk" - // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long - // enough. - // - // Because LIDAR data is naturally "azimuth-major order" and not "ring-major order", such - // implementation is not cache friendly at all, and has negative impact on all the other nodes. - // - // To tackle this issue, the algorithm has been rewritten so that points would be accessed in - // order. To do so, all rings are being processing simultaneously instead of separately. The - // overall logic is still the same. - - // ad-hoc struct to store finished walks information (for isCluster()) - struct WalkInfo - { - size_t id; - int num_points; - float first_point_distance; - float first_point_azimuth; - float last_point_distance; - float last_point_azimuth; - }; - - // ad-hoc struct to keep track of each ring current walk - struct RingWalkInfo - { - WalkInfo first_walk; - WalkInfo current_walk; - }; - - // helper functions - - // check if walk is a valid cluster - const float object_length_threshold2 = object_length_threshold_ * object_length_threshold_; - auto isCluster = [=](const WalkInfo & walk_info) { - // A cluster is a walk which has many points or is long enough - if (walk_info.num_points > num_points_threshold_) return true; - - // Using the law of cosines, the distance between 2 points can be written as: - // |p2-p1|^2 = d1^2 + d2^2 - 2*d1*d2*cos(a) - // where d1 and d2 are the distance attribute of p1 and p2, and 'a' the azimuth diff between - // the 2 points. - const float dist2 = - walk_info.first_point_distance * walk_info.first_point_distance + - walk_info.last_point_distance * walk_info.last_point_distance - - 2 * walk_info.first_point_distance * walk_info.last_point_distance * - std::cos((walk_info.last_point_azimuth - walk_info.first_point_azimuth) * (M_PI / 18000.0)); - return dist2 > object_length_threshold2; - }; - - // check if 2 points belong to the same walk - auto isSameWalk = - [=](float curr_distance, float curr_azimuth, float prev_distance, float prev_azimuth) { - float azimuth_diff = curr_azimuth - prev_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; - return std::max(curr_distance, prev_distance) < - std::min(curr_distance, prev_distance) * distance_ratio_ && - azimuth_diff < 100.f; - }; - - // tmp vectors to keep track of walk/ring state while processing points in order (cache efficient) - std::vector rings; // info for each LiDAR ring - std::vector points_walk_id; // for each input point, the walk index associated with it - std::vector - walks_cluster_status; // for each generated walk, stores whether it is a cluster - - size_t latest_walk_id = -1UL; // ID given to the latest walk created - - // initialize each ring with two empty walks (first and current walk) - rings.resize(max_rings_num_, RingWalkInfo{{-1UL, 0, 0, 0, 0, 0}, {-1UL, 0, 0, 0, 0, 0}}); - // points are initially associated to no walk (-1UL) - points_walk_id.resize(input->width * input->height, -1UL); - walks_cluster_status.reserve( - max_rings_num_ * 2); // In the worst case, this could grow to the number of input points - - int invalid_ring_count = 0; - - // Build walks and classify points - for (const auto & [raw_p, point_walk_id] : - ranges::views::zip(input->data | ranges::views::chunk(input->point_step), points_walk_id)) { - uint16_t ring_idx{}; - float curr_azimuth{}; - float curr_distance{}; - std::memcpy(&ring_idx, &raw_p.data()[ring_offset], sizeof(ring_idx)); - std::memcpy(&curr_azimuth, &raw_p.data()[azimuth_offset], sizeof(curr_azimuth)); - std::memcpy(&curr_distance, &raw_p.data()[distance_offset], sizeof(curr_distance)); - - if (ring_idx >= max_rings_num_) { - // Either the data is corrupted or max_rings_num_ is not set correctly - // Note: point_walk_id == -1 so the point will be filtered out - ++invalid_ring_count; - continue; - } + // walk range: [walk_first_idx, walk_last_idx] + int walk_first_idx = 0; + int walk_last_idx = -1; - auto & ring = rings[ring_idx]; - if (ring.current_walk.id == -1UL) { - // first walk ever for this ring. It is both the first and current walk of the ring. - ring.first_walk = - WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; - ring.current_walk = ring.first_walk; - point_walk_id = latest_walk_id; - continue; - } + for (const auto & indices : ring2indices) { + if (indices.size() < 2) continue; - auto & walk = ring.current_walk; - if (isSameWalk( - curr_distance, curr_azimuth, walk.last_point_distance, walk.last_point_azimuth)) { - // current point is part of previous walk - walk.num_points += 1; - walk.last_point_distance = curr_distance; - walk.last_point_azimuth = curr_azimuth; - point_walk_id = walk.id; - } else { - // previous walk is finished, start a new one - - // check and store whether the previous walks is a cluster - if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); - walks_cluster_status.at(walk.id) = isCluster(walk); - - ring.current_walk = - WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; - point_walk_id = latest_walk_id; - } - } + walk_first_idx = 0; + walk_last_idx = -1; - // So far, we have processed ring points as if rings were not circular. Of course, the last and - // first points of a ring could totally be part of the same walk. When such thing happens, we need - // to merge the two walks - for (auto & ring : rings) { - if (ring.current_walk.id == -1UL) { - continue; - } + for (size_t idx = 0U; idx < indices.size() - 1; ++idx) { + const size_t & current_data_idx = indices[idx]; + const size_t & next_data_idx = indices[idx + 1]; + walk_last_idx = idx; - const auto & walk = ring.current_walk; - if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); - walks_cluster_status.at(walk.id) = isCluster(walk); + // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) - if (ring.first_walk.id == ring.current_walk.id) { - continue; - } + const float & current_azimuth = + *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); + const float & next_azimuth = + *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); + float azimuth_diff = next_azimuth - current_azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; - auto & first_walk = ring.first_walk; - auto & last_walk = ring.current_walk; - - // check if the two walks are connected - if (isSameWalk( - first_walk.first_point_distance, first_walk.first_point_azimuth, - last_walk.last_point_distance, last_walk.last_point_azimuth)) { - // merge - auto combined_num_points = first_walk.num_points + last_walk.num_points; - first_walk.first_point_distance = last_walk.first_point_distance; - first_walk.first_point_azimuth = last_walk.first_point_azimuth; - first_walk.num_points = combined_num_points; - last_walk.last_point_distance = first_walk.last_point_distance; - last_walk.last_point_azimuth = first_walk.last_point_azimuth; - last_walk.num_points = combined_num_points; - - walks_cluster_status.at(first_walk.id) = isCluster(first_walk); - walks_cluster_status.at(last_walk.id) = isCluster(last_walk); - } - } + const float & current_distance = + *reinterpret_cast(&input->data[current_data_idx + distance_offset]); + const float & next_distance = + *reinterpret_cast(&input->data[next_data_idx + distance_offset]); - // finally copy points - output.point_step = sizeof(PointXYZI); - output.data.resize(output.point_step * input->width * input->height); - size_t output_size = 0; - if (transform_info.need_transform) { - for (const auto & [raw_p, point_walk_id] : ranges::views::zip( - input->data | ranges::views::chunk(input->point_step), points_walk_id)) { - // Filter out points on invalid rings and points not in a cluster - if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { - continue; + if ( + std::max(current_distance, next_distance) < + std::min(current_distance, next_distance) * distance_ratio_ && + azimuth_diff < 100.f) { + continue; // Determined to be included in the same walk } - // assume binary representation of input point is compatible with PointXYZ* - PointXYZI out_point; - std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); - - Eigen::Vector4f p(out_point.x, out_point.y, out_point.z, 1); - p = transform_info.eigen_transform * p; - out_point.x = p[0]; - out_point.y = p[1]; - out_point.z = p[2]; - // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI - std::memcpy( - &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); - - std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); - output_size += sizeof(PointXYZI); - } - } else { - for (const auto & [raw_p, point_walk_id] : ranges::views::zip( - input->data | ranges::views::chunk(input->point_step), points_walk_id)) { - // Filter out points on invalid rings and points not in a cluster - if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { - continue; + if (isCluster( + input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), + walk_last_idx - walk_first_idx + 1)) { + for (int i = walk_first_idx; i <= walk_last_idx; i++) { + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + output_ptr->x = p[0]; + output_ptr->y = p[1]; + output_ptr->z = p[2]; + } else { + *output_ptr = *input_ptr; + } + const float & intensity = + *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + output_ptr->intensity = intensity; + + output_size += output.point_step; + } } - PointXYZI out_point; - std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); - // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI - std::memcpy( - &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); - - std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + walk_first_idx = idx + 1; + } - output_size += sizeof(PointXYZI); + if (walk_first_idx > walk_last_idx) continue; + + if (isCluster( + input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), + walk_last_idx - walk_first_idx + 1)) { + for (int i = walk_first_idx; i <= walk_last_idx; i++) { + auto output_ptr = reinterpret_cast(&output.data[output_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + output_ptr->x = p[0]; + output_ptr->y = p[1]; + output_ptr->z = p[2]; + } else { + *output_ptr = *input_ptr; + } + const float & intensity = + *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + output_ptr->intensity = intensity; + + output_size += output.point_step; + } } } + output.data.resize(output_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -332,12 +196,6 @@ void RingOutlierFilterComponent::faster_filter( sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); - if (invalid_ring_count > 0) { - RCLCPP_WARN( - get_logger(), "%d points had ring index over max_rings_num (%d) and have been ignored.", - invalid_ring_count, max_rings_num_); - } - // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); @@ -346,6 +204,14 @@ void RingOutlierFilterComponent::faster_filter( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 5d261f42391e9..8796477f3d9ae 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -274,6 +274,9 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() for (const auto & e : cloud_stdmap_) { transformed_clouds[e.first] = nullptr; if (e.second != nullptr) { + if (e.second->data.size() == 0) { + continue; + } pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); } } @@ -288,11 +291,22 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() // Step2. Calculate compensation transform and concatenate with the oldest stamp for (const auto & e : cloud_stdmap_) { if (e.second != nullptr) { + // transform pointcloud sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + if (e.second->data.size() == 0) { + // gather transformed clouds + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; + transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + continue; + } + // transform pointcloud to output frame transformPointCloud(e.second, transformed_cloud_ptr); - // calculate transforms to oldest stamp + // calculate transforms to oldest stamp and transform pointcloud to oldest stamp Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); rclcpp::Time transformed_stamp = rclcpp::Time(e.second->header.stamp); for (const auto & stamp : pc_stamps) { @@ -301,15 +315,15 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform; transformed_stamp = std::min(transformed_stamp, stamp); } - sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( - new sensor_msgs::msg::PointCloud2()); pcl_ros::transformPointCloud( adjust_to_old_data_transform, *transformed_cloud_ptr, *transformed_delay_compensated_cloud_ptr); + // gather transformed clouds transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + } else { not_subscribed_topic_names_.insert(e.first); } @@ -414,7 +428,9 @@ void PointCloudDataSynchronizerComponent::cloud_callback( std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); - convertToXYZICloud(input, xyzi_input_ptr); + if (input->data.size() > 0) { + convertToXYZICloud(input, xyzi_input_ptr); + } const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index 2360e8b33901d..a9d4fe7ebda8f 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -5,6 +5,9 @@ radar_scan_to_pointcloud2 Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index a15aa43d71cf6..05453ee9cb9e0 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -5,6 +5,9 @@ radar_static_pointcloud_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_threshold_filter/package.xml b/sensing/radar_threshold_filter/package.xml index 00bb530567dc4..6b81f225db971 100644 --- a/sensing/radar_threshold_filter/package.xml +++ b/sensing/radar_threshold_filter/package.xml @@ -5,6 +5,9 @@ radar_threshold_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index 517a3f1df72f1..56cbdb98fdbc8 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -28,13 +28,21 @@ rclcpp_components_register_node(radar_tracks_noise_filter_node_component # Tests if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(radar_tracks_noise_filter ${test_files}) + + target_link_libraries(radar_tracks_noise_filter + radar_tracks_noise_filter_node_component + ) endif() # Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml b/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml new file mode 100644 index 0000000000000..0d7ddd345b03d --- /dev/null +++ b/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_y_threshold: 7.0 diff --git a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp index 9b329da3e5579..2ff4025cc64bc 100644 --- a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp +++ b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp @@ -57,6 +57,7 @@ class RadarTrackCrossingNoiseFilterNode : public rclcpp::Node // Parameter NodeParam node_param_{}; +public: // Core bool isNoise(const RadarTrack & radar_track); }; diff --git a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml index f38fe36b50af4..028ca5de6a220 100644 --- a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml +++ b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml @@ -1,17 +1,13 @@ - - - - + - - + diff --git a/sensing/radar_tracks_noise_filter/package.xml b/sensing/radar_tracks_noise_filter/package.xml index 3af9658535687..8420b7a174edf 100644 --- a/sensing/radar_tracks_noise_filter/package.xml +++ b/sensing/radar_tracks_noise_filter/package.xml @@ -6,6 +6,9 @@ radar_tracks_noise_filter Sathshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Sathshi Tanaka diff --git a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp new file mode 100644 index 0000000000000..aa08260dee2e7 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 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 "radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" + +#include + +#include + +std::shared_ptr get_node( + float velocity_y_threshold) +{ + rclcpp::NodeOptions node_options; + node_options.parameter_overrides({ + {"node_params.is_amplitude_filter", true}, + {"velocity_y_threshold", velocity_y_threshold}, + }); + + auto node = + std::make_shared(node_options); + return node; +} + +radar_msgs::msg::RadarTrack getRadarTrack(float velocity_x, float velocity_y) +{ + radar_msgs::msg::RadarTrack radar_track; + geometry_msgs::msg::Vector3 vector_msg; + vector_msg.x = velocity_x; + vector_msg.y = velocity_y; + vector_msg.z = 0.0; + radar_track.velocity = vector_msg; + return radar_track; +} + +TEST(RadarTracksNoiseFilter, isNoise) +{ + using radar_msgs::msg::RadarTrack; + using radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; + rclcpp::init(0, nullptr); + { + float velocity_node_threshold = 0.0; + float y_velocity = 0.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 100.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = -10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } +} diff --git a/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp new file mode 100644 index 0000000000000..55032a7b62e79 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 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 + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/simulator/fault_injection/config/fault_injection.param.yaml b/simulator/fault_injection/config/fault_injection.param.yaml index 1a57b852f7361..ac02442d70b4d 100644 --- a/simulator/fault_injection/config/fault_injection.param.yaml +++ b/simulator/fault_injection/config/fault_injection.param.yaml @@ -4,7 +4,7 @@ vehicle_is_out_of_lane: "lane_departure" trajectory_deviation_is_high: "trajectory_deviation" localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_accuracy" + localization_accuracy_is_low: "localization_error_ellipse" map_version_is_different: "map_version" trajectory_is_invalid: "trajectory_point_validation" cpu_temperature_is_high: "CPU Temperature" diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index ed2b06ee1f44d..d2ed19708c5c5 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -40,18 +40,17 @@ The purpose of this simulator is for the integration test of planning and contro ### Common Parameters -| Name | Type | Description | Default value | -| :--------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | -| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" | -| origin_frame_id | string | set to the frame_id in output tf | "odom" | -| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | -| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | -| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | -| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | -| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | -| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | -| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | -| measurement_steer_bias | double | Measurement bias for steering angle | 0.0 | +| Name | Type | Description | Default value | +| :-------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | +| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" | +| origin_frame_id | string | set to the frame_id in output tf | "odom" | +| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `input/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | +| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | +| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | +| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | +| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | +| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | +| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | ### Vehicle Model Parameters @@ -82,6 +81,7 @@ The table below shows which models correspond to what parameters. The model name | vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | 7.0 | [m/ss] | | steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | 1.0 | [rad] | | steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | 5.0 | [rad/s] | +| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | 0.0 | [rad] | | debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | 1.0 | [-] | | debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 1.0 | [-] | | acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | - | [-] | 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 4c3a3aed24d90..58914d19552df 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 @@ -195,9 +195,6 @@ 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 - /* measurement bias */ - double measurement_steer_bias_ = 0.0; //!< @brief measurement bias for steering measurement - DeltaTime delta_time_{}; //!< @brief to calculate delta time MeasurementNoiseGenerator measurement_noise_{}; //!< @brief for measurement noise diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index 92129541ff891..f43abd8572874 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -40,14 +40,15 @@ class SimModelDelaySteerAcc : public SimModelInterface * @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_dead_band dead band for steering angle [rad] + * @param [in] steer_bias steering bias [rad] * @param [in] debug_acc_scaling_factor scaling factor for accel command * @param [in] debug_steer_scaling_factor scaling factor for steering command */ SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor); /** * @brief default destructor @@ -84,6 +85,7 @@ class SimModelDelaySteerAcc : public SimModelInterface 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_dead_band_; //!< @brief dead band for steering angle [rad] + const double steer_bias_; //!< @brief steering angle bias [rad] const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 376ee55f1aa5e..35b95bf4b1ae5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -40,14 +40,15 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @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_dead_band dead band for steering angle [rad] + * @param [in] steer_bias steering bias [rad] * @param [in] debug_acc_scaling_factor scaling factor for accel command * @param [in] debug_steer_scaling_factor scaling factor for steering command */ SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor); + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor); /** * @brief default destructor @@ -84,6 +85,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface 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_dead_band_; //!< @brief dead band for steering angle [rad] + const double steer_bias_; //!< @brief steering angle bias [rad] const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command 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 fecd3c2bc8be4..d7ca032aa6c48 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 @@ -92,12 +92,13 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface * @param [in] acc_time_constant time constant for 1D model of accel 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] path path to csv file for acceleration conversion map */ SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, std::string path); + double steer_time_constant, double steer_bias, std::string path); /** * @brief default destructor @@ -135,6 +136,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface const double acc_time_constant_; //!< @brief time constant for accel 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 /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp index 5c1ec55d522bf..61a9e8d0a5643 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -43,11 +43,12 @@ class SimModelDelaySteerVel : public SimModelInterface * @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_dead_band dead band for steering angle [rad] + * @param [in] steer_bias steering bias [rad] */ SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band); + double steer_time_constant, double steer_dead_band, double steer_bias); /** * @brief destructor @@ -86,6 +87,7 @@ class SimModelDelaySteerVel : public SimModelInterface const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics const double steer_dead_band_; //!< @brief dead band for steering angle [rad] + const double steer_bias_; //!< @brief steering angle bias [rad] /** * @brief set queue buffer for input command 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 37ec5b33014a7..26be4841e2f2a 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 @@ -102,7 +102,6 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link"); origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); add_measurement_noise_ = declare_parameter("add_measurement_noise", false); - measurement_steer_bias_ = declare_parameter("measurement_steer_bias", 0.0); simulate_motion_ = declare_parameter("initial_engage_state"); enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false); @@ -232,6 +231,8 @@ void SimplePlanningSimulator::initialize_vehicle_model() const double steer_time_delay = declare_parameter("steer_time_delay", 0.24); const double steer_time_constant = declare_parameter("steer_time_constant", 0.27); const double steer_dead_band = declare_parameter("steer_dead_band", 0.0); + const double steer_bias = declare_parameter("steer_bias", 0.0); + const double debug_acc_scaling_factor = declare_parameter("debug_acc_scaling_factor", 1.0); const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -250,19 +251,20 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, + steer_bias); } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, - debug_acc_scaling_factor, debug_steer_scaling_factor); + steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, - debug_acc_scaling_factor, debug_steer_scaling_factor); + steer_bias, debug_acc_scaling_factor, debug_steer_scaling_factor); } else if (vehicle_model_type_str == "DELAY_STEER_MAP_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_MAP_ACC_GEARED; const std::string acceleration_map_path = @@ -277,7 +279,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() } vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_bias, acceleration_map_path); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); @@ -383,9 +385,6 @@ void SimplePlanningSimulator::on_timer() add_measurement_noise(current_odometry_, current_velocity_, current_steer_); } - // add measurement bias - current_steer_.steering_tire_angle += measurement_steer_bias_; - // add estimate covariance { using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 670671165de60..6a9b5c65d4760 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -19,8 +19,8 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor) + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -33,6 +33,7 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias), debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { @@ -69,7 +70,7 @@ double SimModelDelaySteerAcc::getWz() } double SimModelDelaySteerAcc::getSteer() { - return state_(IDX::STEER); + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerAcc::update(const double & dt) { @@ -111,7 +112,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_; const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_; - const double steer_diff = steer - steer_des; + const double steer_diff = getSteer() - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { return steer_diff - steer_dead_band_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index d72b8bf116d77..10e6a97d703cd 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -21,8 +21,8 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band, double debug_acc_scaling_factor, - double debug_steer_scaling_factor) + double steer_time_constant, double steer_dead_band, double steer_bias, + double debug_acc_scaling_factor, double debug_steer_scaling_factor) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -35,6 +35,7 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias), debug_acc_scaling_factor_(std::max(debug_acc_scaling_factor, 0.0)), debug_steer_scaling_factor_(std::max(debug_steer_scaling_factor, 0.0)) { @@ -68,10 +69,12 @@ double SimModelDelaySteerAccGeared::getAx() double SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; + ; } double SimModelDelaySteerAccGeared::getSteer() { - return state_(IDX::STEER); + // return measured values with bias added to actual values + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerAccGeared::update(const double & dt) { @@ -119,7 +122,10 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_) * debug_acc_scaling_factor_; const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_) * debug_steer_scaling_factor_; - const double steer_diff = steer - steer_des; + // NOTE: `steer_des` is calculated by control from measured values. getSteer() also gets the + // measured value. The steer_rate used in the motion calculation is obtained from these + // differences. + const double steer_diff = getSteer() - steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { return steer_diff - steer_dead_band_; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index b04a10667adcf..72273f0b21ec2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -21,7 +21,7 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, std::string path) + double steer_time_constant, double steer_bias, std::string path) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -33,6 +33,7 @@ SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_bias_(steer_bias), path_(path) { initializeInputQueue(dt); @@ -69,7 +70,7 @@ double SimModelDelaySteerMapAccGeared::getWz() } double SimModelDelaySteerMapAccGeared::getSteer() { - return state_(IDX::STEER); + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerMapAccGeared::update(const double & dt) { @@ -113,7 +114,7 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( const double steer = state(IDX::STEER); const double acc_des = std::clamp(input(IDX_U::ACCX_DES), -vx_rate_lim_, vx_rate_lim_); const double steer_des = std::clamp(input(IDX_U::STEER_DES), -steer_lim_, steer_lim_); - double steer_rate = -(steer - steer_des) / steer_time_constant_; + double steer_rate = -(getSteer() - steer_des) / steer_time_constant_; steer_rate = std::clamp(steer_rate, -steer_rate_lim_, steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 6d155ee921107..1ee08fad566f5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -19,7 +19,7 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double steer_dead_band) + double steer_time_constant, double steer_dead_band, double steer_bias) : SimModelInterface(5 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -31,7 +31,8 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - steer_dead_band_(steer_dead_band) + steer_dead_band_(steer_dead_band), + steer_bias_(steer_bias) { initializeInputQueue(dt); } @@ -66,7 +67,7 @@ double SimModelDelaySteerVel::getWz() } double SimModelDelaySteerVel::getSteer() { - return state_(IDX::STEER); + return state_(IDX::STEER) + steer_bias_; } void SimModelDelaySteerVel::update(const double & dt) { @@ -109,7 +110,7 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - const double steer_diff = steer - delay_steer_des; + const double steer_diff = getSteer() - delay_steer_des; const double steer_diff_with_dead_band = std::invoke([&]() { if (steer_diff > steer_dead_band_) { return steer_diff - steer_dead_band_; diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index 55af6ab2d2c55..9ed82304036cc 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -24,9 +24,9 @@ contains: ["ndt_scan_matcher"] timeout: 1.0 - localization_accuracy: + localization_error_ellipse: type: diagnostic_aggregator/GenericAnalyzer - path: localization_accuracy + path: localization_error_ellipse contains: ["localization: localization_error_monitor"] timeout: 1.0 diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 1778f6594f0c3..2b58f5b42c0be 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/localization/performance_monitoring/localization_accuracy: default + /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 9784259490ec2..f1031444394d0 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/localization/performance_monitoring/localization_accuracy: default + # /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default