diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 12ad752b3ef29..6b4daaa4b1773 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ ### Copied from .github/CODEOWNERS-manual ### ### Automatically generated from package.xml ### -common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +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 @@ -9,9 +9,9 @@ common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wak 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/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp -common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp -common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_tools/** isamu.takagi@tier4.jp +common/component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/geography_utils/** koji.minoda@tier4.jp @@ -24,7 +24,7 @@ common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.sa 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/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 kahhooi.tan@tier4.jp makoto.yabuta@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/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 @@ -32,16 +32,16 @@ common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp 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 kahhooi.tan@tier4.jp makoto.yabuta@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_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 common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp -common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp -common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp +common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp @@ -54,7 +54,7 @@ common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp -common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +common/traffic_light_utils/** mingyu.li@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp 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 @@ -76,9 +76,9 @@ 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 -launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@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/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@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_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 @@ -86,29 +86,29 @@ launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp launch/tier4_system_launch/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp -localization/geo_pose_projector/** koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/ar_tag_based_localizer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/landmark_manager/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/localization_util/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/pose_instability_detector/** 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/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/tree_structured_parzen_estimator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/twist2accel/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_monitor/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -map/map_height_fitter/** isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp -map/map_projection_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_tf_generator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/tree_structured_parzen_estimator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_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_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 perception/bytetrack/** manato.hirabayashi@tier4.jp perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @@ -156,6 +156,7 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_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_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 @@ -166,6 +167,7 @@ planning/behavior_path_start_planner_module/** kosuke.takeuchi@tier4.jp kyoichi. 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 +planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.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 planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -173,7 +175,7 @@ planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp @@ -223,11 +225,11 @@ simulator/fault_injection/** keisuke.shima@tier4.jp simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp -system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/component_state_monitor/** isamu.takagi@tier4.jp +system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp diff --git a/common/autoware_ad_api_specs/package.xml b/common/autoware_ad_api_specs/package.xml index d4d82faf4ee78..0d14ccff758b0 100644 --- a/common/autoware_ad_api_specs/package.xml +++ b/common/autoware_ad_api_specs/package.xml @@ -5,8 +5,6 @@ 0.0.0 The autoware_ad_api_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt index dd5f88d4caae8..8d0469e78c3ac 100644 --- a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt +++ b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt @@ -13,18 +13,18 @@ set(OD_PLUGIN_LIB_SRC ) set(OD_PLUGIN_LIB_HEADERS - include/visibility_control.hpp + include/autoware_auto_perception_rviz_plugin/visibility_control.hpp ) set(OD_PLUGIN_LIB_HEADERS_TO_WRAP - include/object_detection/detected_objects_display.hpp - include/object_detection/tracked_objects_display.hpp - include/object_detection/predicted_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp ) set(COMMON_HEADERS - include/common/color_alpha_property.hpp - include/object_detection/object_polygon_detail.hpp - include/object_detection/object_polygon_display_base.hpp + include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp + include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp ) set(COMMON_SRC diff --git a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp similarity index 84% rename from common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp index 8c43192a26bd6..10dc46e55ec70 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp @@ -11,13 +11,14 @@ // 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 COMMON__COLOR_ALPHA_PROPERTY_HPP_ -#define COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ + +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" #include #include #include -#include #include @@ -55,4 +56,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty } // namespace rviz_plugins } // namespace autoware -#endif // COMMON__COLOR_ALPHA_PROPERTY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp similarity index 76% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp index 67dac25c796bb..97479fb68ca9b 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -11,10 +11,10 @@ // 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_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -43,4 +43,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp similarity index 78% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index b661010c19a4d..5f9da8531d2ab 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. /// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class -#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ -#define OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" + +#include +#include #include #include -#include #include #include @@ -83,13 +86,15 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::Sha get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available = true); /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame @@ -110,8 +115,14 @@ get_uuid_marker_ptr( const std_msgs::msg::ColorRGBA & color_rgba); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); +get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & confidence_interval_coefficient, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( @@ -128,6 +139,23 @@ get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, @@ -139,14 +167,33 @@ get_path_confidence_marker_ptr( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( + const double start_angle, const double end_angle, const double radius, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points); + +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); @@ -238,4 +285,4 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp similarity index 77% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 223611520b2fc..4290fdff49bb3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -11,19 +11,19 @@ // 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_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "common/color_alpha_property.hpp" +#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" +#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" -#include #include #include #include #include #include #include -#include #include #include @@ -64,14 +64,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, - m_display_pose_with_covariance_property{ - "Display PoseWithCovariance", true, "Enable/disable pose with covariance visualization", - this}, m_display_velocity_text_property{ "Display Velocity", true, "Enable/disable velocity text visualization", this}, m_display_acceleration_text_property{ "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, + m_display_pose_covariance_property{ + "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this}, + m_display_yaw_covariance_property{ + "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this}, m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, + m_display_twist_covariance_property{ + "Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, + m_display_yaw_rate_property{ + "Display Yaw Rate", false, "Enable/disable yaw rate visualization", this}, + m_display_yaw_rate_covariance_property{ + "Display Yaw Rate Covariance", false, "Enable/disable yaw rate covariance visualization", + this}, m_display_predicted_paths_property{ "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, m_display_path_confidence_property{ @@ -96,6 +104,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this); m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); + // Confidence interval property + m_confidence_interval_property = new rviz_common::properties::EnumProperty( + "Confidence Interval", "95%", "Confidence interval of state estimations.", this); + m_confidence_interval_property->addOption("70%", 0); + m_confidence_interval_property->addOption("85%", 1); + m_confidence_interval_property->addOption("95%", 2); + m_confidence_interval_property->addOption("99%", 3); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { @@ -170,14 +185,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const ClassificationContainerT & labels, const double & line_width) const + const ClassificationContainerT & labels, const double & line_width, + const bool & is_orientation_available) const { const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); if (m_display_type_property->getOptionInt() == 0) { - return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width); + return detail::get_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); } else if (m_display_type_property->getOptionInt() == 1) { return detail::get_2d_shape_marker_ptr( - shape_msg, centroid, orientation, color_rgba, line_width); + shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available); } else { return std::nullopt; } @@ -187,7 +204,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available); /// \brief Convert given shape msg into a Marker to visualize label name /// \tparam ClassificationContainerT List type with ObjectClassificationMsg @@ -235,11 +253,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - std::optional get_pose_with_covariance_marker_ptr( + std::optional get_pose_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const { - if (m_display_pose_with_covariance_property.getBool()) { - return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance); + if (m_display_pose_covariance_property.getBool()) { + return detail::get_pose_covariance_marker_ptr(pose_with_covariance, get_confidence_region()); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & line_width) const + { + if (m_display_yaw_covariance_property.getBool()) { + return detail::get_yaw_covariance_marker_ptr( + pose_with_covariance, length, get_confidence_interval(), line_width); } else { return std::nullopt; } @@ -283,6 +313,44 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + std::optional get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const + { + if (m_display_twist_covariance_property.getBool()) { + return detail::get_twist_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, get_confidence_region()); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_yaw_rate_property.getBool()) { + return detail::get_yaw_rate_marker_ptr( + pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + + std::optional get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_yaw_rate_covariance_property.getBool()) { + return detail::get_yaw_rate_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, get_confidence_interval(), line_width); + } else { + return std::nullopt; + } + } + std::optional get_predicted_path_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, const autoware_auto_perception_msgs::msg::Shape & shape, @@ -405,6 +473,46 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() { return m_line_width_property.getFloat(); } + double get_confidence_interval() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 70% + return 1.036; + case 1: + // 85% + return 1.440; + case 2: + // 95% + return 1.960; + case 3: + // 99% + return 2.576; + default: + return 1.960; + } + } + + double get_confidence_region() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 70% + return 1.552; + case 1: + // 85% + return 1.802; + case 2: + // 95% + return 2.448; + case 3: + // 99% + return 3.035; + default: + return 2.448; + } + } + private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; @@ -416,18 +524,28 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::EnumProperty * m_display_type_property; // Property to choose simplicity of visualization polygon rviz_common::properties::EnumProperty * m_simple_visualize_mode_property; + // Property to set confidence interval of state estimations + rviz_common::properties::EnumProperty * m_confidence_interval_property; // Property to enable/disable label visualization rviz_common::properties::BoolProperty m_display_label_property; // Property to enable/disable uuid visualization rviz_common::properties::BoolProperty m_display_uuid_property; - // Property to enable/disable pose with covariance visualization - rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; // Property to enable/disable velocity text visualization rviz_common::properties::BoolProperty m_display_velocity_text_property; // Property to enable/disable acceleration text visualization rviz_common::properties::BoolProperty m_display_acceleration_text_property; + // Property to enable/disable pose with covariance visualization + rviz_common::properties::BoolProperty m_display_pose_covariance_property; + // Property to enable/disable yaw covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_covariance_property; // Property to enable/disable twist visualization rviz_common::properties::BoolProperty m_display_twist_property; + // Property to enable/disable twist covariance visualization + rviz_common::properties::BoolProperty m_display_twist_covariance_property; + // Property to enable/disable yaw rate visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_property; + // Property to enable/disable yaw rate covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_covariance_property; // Property to enable/disable predicted paths visualization rviz_common::properties::BoolProperty m_display_predicted_paths_property; // Property to enable/disable predicted path confidence visualization @@ -446,4 +564,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp similarity index 92% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 2896286970217..775c18db6ba5c 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -11,10 +11,10 @@ // 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_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -153,4 +153,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp similarity index 89% rename from common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index a4cf8a703dff1..4e86a5ee93fd8 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -11,10 +11,10 @@ // 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_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#define OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" #include @@ -114,4 +114,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp similarity index 89% rename from common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp rename to common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp index bce7351572375..47cb8383fdada 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/visibility_control.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef VISIBILITY_CONTROL_HPP_ -#define VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) #if defined(AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_BUILDING_DLL) || \ @@ -40,4 +40,4 @@ #error "Unsupported Build Configuration" #endif // defined(_WINDOWS) -#endif // VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp index 57a42c95f9d34..b3e542a02243b 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/common/color_alpha_property.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "common/color_alpha_property.hpp" +#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp" #include diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 5dcdce791c585..53e935fa1850a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp" #include @@ -38,7 +38,9 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, object.kinematics.pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -57,6 +59,27 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(label_marker_ptr); } + // Get marker for pose covariance + auto pose_with_covariance_marker = + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); + if (pose_with_covariance_marker) { + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + // Get marker for existence probability geometry_msgs::msg::Point existence_probability_position; existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; @@ -97,6 +120,38 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) twist_marker_ptr->id = id++; add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw rate covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.3); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } } } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index d87541b7840a9..11a0bbbe57d53 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License.. +#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" + #include #include -#include #include @@ -100,17 +101,16 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; - geometry_msgs::msg::Point pt_s; - pt_s.x = 0.0; - pt_s.y = 0.0; - pt_s.z = 0.0; - marker_ptr->points.push_back(pt_s); - - geometry_msgs::msg::Point pt_e; - pt_e.x = twist_with_covariance.twist.linear.x; - pt_e.y = twist_with_covariance.twist.linear.y; - pt_e.z = twist_with_covariance.twist.linear.z; - marker_ptr->points.push_back(pt_e); + // velocity line + geometry_msgs::msg::Point point; + point.x = 0.0; + point.y = 0.0; + point.z = 0.0; + marker_ptr->points.push_back(point); + point.x = twist_with_covariance.twist.linear.x; + point.y = twist_with_covariance.twist.linear.y; + point.z = twist_with_covariance.twist.linear.z; + marker_ptr->points.push_back(point); marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; @@ -121,6 +121,168 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; + marker_ptr->ns = std::string("twist covariance"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // position is the tip of the velocity vector + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double pos_yaw_angle = 2.0 * std::atan2( + pose_with_covariance.pose.orientation.z, + pose_with_covariance.pose.orientation.w); // [rad] + marker_ptr->pose.position.x += velocity * std::cos(pos_yaw_angle + velocity_angle); + marker_ptr->pose.position.y += velocity * std::sin(pos_yaw_angle + velocity_angle); + + // velocity covariance + // extract eigen values and eigen vectors + Eigen::Matrix2d eigen_twist_covariance; + eigen_twist_covariance << twist_with_covariance.covariance[0], + twist_with_covariance.covariance[1], twist_with_covariance.covariance[6], + twist_with_covariance.covariance[7]; + double phi, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_twist_covariance, sigma1, sigma2, phi); + phi = pos_yaw_angle + phi; + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 1.0 / area); + alpha = std::max(0.1, alpha); + + // ellipse orientation + marker_ptr->pose.orientation.x = 0.0; + marker_ptr->pose.orientation.y = 0.0; + marker_ptr->pose.orientation.z = std::sin(phi / 2.0); + marker_ptr->pose.orientation.w = std::cos(phi / 2.0); + + // ellipse size + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; + marker_ptr->scale.z = 0.05; + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = alpha; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.2; + marker_ptr->color.b = 0.4; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw rate"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // yaw rate + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double yaw_mark_length = velocity * 0.8; + + geometry_msgs::msg::Point point; + // first point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); + // yaw rate arc + calc_arc_line_strip( + velocity_angle, velocity_angle + yaw_rate, yaw_mark_length, marker_ptr->points); + // last point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = 0.9; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.0; + marker_ptr->color.b = 0.0; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("yaw rate covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // yaw rate covariance + const double yaw_rate_covariance = twist_with_covariance.covariance[35]; + const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * confidence_interval_coefficient; + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double yaw_mark_length = velocity * 0.8; + const double bar_width = std::max(velocity * 0.05, 0.1); + const double velocity_yaw_angle = velocity_angle + yaw_rate; + const double velocity_yaw_p_sigma_angle = velocity_yaw_angle + yaw_rate_sigma; + const double velocity_yaw_n_sigma_angle = velocity_yaw_angle - yaw_rate_sigma; + + const double point_list[7][3] = { + {yaw_mark_length * std::cos(velocity_yaw_angle), yaw_mark_length * std::sin(velocity_yaw_angle), + 0}, + {yaw_mark_length * std::cos(velocity_yaw_p_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_p_sigma_angle), 0}, + {yaw_mark_length * std::cos(velocity_yaw_n_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {0, 2}, + {3, 4}, + {5, 6}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, marker_ptr->points); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = 0.9; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.2; + marker_ptr->color.b = 0.4; + + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba) @@ -163,47 +325,117 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( return marker_ptr; } -visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) +void calc_arc_line_strip( + const double start_angle, const double end_angle, const double radius, + std::vector & points) +{ + geometry_msgs::msg::Point point; + // arc points + const double maximum_delta_angle = 10.0 * M_PI / 180.0; + const int num_points = + std::max(3, static_cast(std::abs(end_angle - start_angle) / maximum_delta_angle)); + for (int i = 0; i < num_points; ++i) { + const double angle = start_angle + (end_angle - start_angle) * static_cast(i) / + static_cast(num_points - 1); + point.x = radius * std::cos(angle); + point.y = radius * std::sin(angle); + point.z = 0; + points.push_back(point); + } +} + +void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw) +{ + Eigen::SelfAdjointEigenSolver solver(matrix); + Eigen::Vector2d eigen_values = solver.eigenvalues(); + // eigen values + sigma1 = std::sqrt(eigen_values.x()); + sigma2 = std::sqrt(eigen_values.y()); + // orientation of covariance ellipse + Eigen::Vector2d e1 = solver.eigenvectors().col(0); + yaw = std::atan2(e1.y(), e1.x()); +} + +visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient) { auto marker_ptr = std::make_shared(); - marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; marker_ptr->ns = std::string("position covariance"); - marker_ptr->scale.x = 0.03; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; + + // position covariance + // extract eigen values and eigen vectors + Eigen::Matrix2d eigen_pose_covariance; + eigen_pose_covariance << pose_with_covariance.covariance[0], pose_with_covariance.covariance[1], + pose_with_covariance.covariance[6], pose_with_covariance.covariance[7]; + double yaw, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_pose_covariance, sigma1, sigma2, yaw); + + // ellipse orientation marker_ptr->pose.orientation.x = 0.0; marker_ptr->pose.orientation.y = 0.0; - marker_ptr->pose.orientation.z = 0.0; - marker_ptr->pose.orientation.w = 1.0; + marker_ptr->pose.orientation.z = std::sin(yaw / 2.0); + marker_ptr->pose.orientation.w = std::cos(yaw / 2.0); + + // ellipse size + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; + marker_ptr->scale.z = 0.05; + + // ellipse color density + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 3.0 / area); + alpha = std::max(0.1, alpha); + + // marker configuration + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = alpha; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 1.0; + marker_ptr->color.b = 1.0; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & confidence_interval_coefficient, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; geometry_msgs::msg::Point point; - Eigen::Matrix2d eigen_pose_with_covariance; - eigen_pose_with_covariance << pose_with_covariance.covariance[0], - pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], - pose_with_covariance.covariance[7]; - Eigen::SelfAdjointEigenSolver solver(eigen_pose_with_covariance); - double sigma1 = 2.448 * std::sqrt(solver.eigenvalues().x()); // 2.448 sigma is 95% - double sigma2 = 2.448 * std::sqrt(solver.eigenvalues().y()); // 2.448 sigma is 95% - Eigen::Vector2d e1 = solver.eigenvectors().col(0); - Eigen::Vector2d e2 = solver.eigenvectors().col(1); - point.x = -e1.x() * sigma1; - point.y = -e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = e1.x() * sigma1; - point.y = e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = -e2.x() * sigma2; - point.y = -e2.y() * sigma2; + + // orientation covariance + double yaw_vector_length = std::max(length, 1.0); + double yaw_sigma = + std::sqrt(pose_with_covariance.covariance[35]) * confidence_interval_coefficient; + // get arc points + if (yaw_sigma > M_PI) { + yaw_vector_length = 1.0; + } + // first point + point.x = 0; + point.y = 0; point.z = 0; marker_ptr->points.push_back(point); - point.x = e2.x() * sigma2; - point.y = e2.y() * sigma2; + // arc points + calc_arc_line_strip(-yaw_sigma, yaw_sigma, yaw_vector_length, marker_ptr->points); + // last point + point.x = 0; + point.y = 0; point.z = 0; marker_ptr->points.push_back(point); + + // marker configuration marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); - marker_ptr->color.a = 0.999; + marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; marker_ptr->color.b = 1.0; @@ -262,7 +494,8 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -271,6 +504,9 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_bounding_box_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_cylinder_line_list(shape_msg, marker_ptr->points); @@ -294,7 +530,8 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, + const bool & is_orientation_available) { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); @@ -303,6 +540,9 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + if (is_orientation_available) { + calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points); + } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points); @@ -323,163 +563,120 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( return marker_ptr; } +void calc_line_list_from_points( + const double point_list[][3], const int point_pairs[][2], const int & num_pairs, + std::vector & points) +{ + geometry_msgs::msg::Point point; + for (int i = 0; i < num_pairs; ++i) { + point.x = point_list[point_pairs[i][0]][0]; + point.y = point_list[point_pairs[i][0]][1]; + point.z = point_list[point_pairs[i][0]][2]; + points.push_back(point); + point.x = point_list[point_pairs[i][1]][0]; + point.y = point_list[point_pairs[i][1]][1]; + point.z = point_list[point_pairs[i][1]][2]; + points.push_back(point); + } +} + void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; geometry_msgs::msg::Point point; - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - // up surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = shape.dimensions.z / 2.0; - points.push_back(point); - - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // bounding box corner points + // top and bottom surface, clockwise + const double point_list[8][3] = { + {length_half, width_half, height_half}, {length_half, -width_half, height_half}, + {-length_half, -width_half, height_half}, {-length_half, width_half, height_half}, + {length_half, width_half, -height_half}, {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, {-length_half, width_half, -height_half}, + }; + const int point_pairs[12][2] = { + {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6}, {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7}, + }; + calc_line_list_from_points(point_list, point_pairs, 12, points); +} + +void calc_bounding_box_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + // direction triangle + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double triangle_size_half = shape.dimensions.y / 1.4; + geometry_msgs::msg::Point point; + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, height_half}, + {length_half - triangle_size_half, width_half, height_half}, + {length_half - triangle_size_half, -width_half, height_half}, + {length_half, 0, -height_half}, + {length_half, width_half, height_half}, + {length_half, -width_half, height_half}, + }; + const int point_pairs[5][2] = { + {0, 1}, {1, 2}, {0, 2}, {3, 4}, {3, 5}, + }; + calc_line_list_from_points(point_list, point_pairs, 5, points); } void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; geometry_msgs::msg::Point point; - // down surface - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = -shape.dimensions.x / 2.0; - point.y = shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - - point.x = shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); - point.x = -shape.dimensions.x / 2.0; - point.y = -shape.dimensions.y / 2.0; - point.z = -shape.dimensions.z / 2.0; - points.push_back(point); + + // bounding box corner points + // top surface, clockwise + const double point_list[4][3] = { + {length_half, width_half, -height_half}, + {length_half, -width_half, -height_half}, + {-length_half, -width_half, -height_half}, + {-length_half, width_half, -height_half}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {1, 2}, + {2, 3}, + {3, 0}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, points); +} + +void calc_2d_bounding_box_bottom_direction_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double triangle_size_half = shape.dimensions.y / 1.4; + geometry_msgs::msg::Point point; + + // triangle-shaped direction indicator + const double point_list[6][3] = { + {length_half, 0, -height_half}, + {length_half - triangle_size_half, width_half, -height_half}, + {length_half - triangle_size_half, -width_half, -height_half}, + }; + const int point_pairs[3][2] = { + {0, 1}, + {1, 2}, + {0, 2}, + }; + calc_line_list_from_points(point_list, point_pairs, 3, points); } void calc_cylinder_line_list( diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 438d70f052b4a..24b21a15732c3 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp" #include #include @@ -77,12 +77,12 @@ std::vector PredictedObjectsDisplay: auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification, - get_line_width()); + get_line_width(), true); if (shape_marker) { - auto shape_marker_ptr = shape_marker.value(); - shape_marker_ptr->header = msg->header; - shape_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(shape_marker_ptr); + auto marker_ptr = shape_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for label @@ -90,10 +90,10 @@ std::vector PredictedObjectsDisplay: object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification); if (label_marker) { - auto label_marker_ptr = label_marker.value(); - label_marker_ptr->header = msg->header; - label_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(label_marker_ptr); + auto marker_ptr = label_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for id @@ -111,14 +111,25 @@ std::vector PredictedObjectsDisplay: markers.push_back(id_marker_ptr); } - // Get marker for pose with covariance + // Get marker for pose covariance auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); + get_pose_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for existence probability @@ -181,6 +192,39 @@ std::vector PredictedObjectsDisplay: markers.push_back(twist_marker_ptr); } + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for twist covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.3); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + // Add marker for each candidate path int32_t path_count = 0; for (const auto & predicted_path : object.kinematics.predicted_paths) { diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 75b094b49a762..504b51f850444 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp" #include @@ -55,11 +55,13 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) for (const auto & object : msg->objects) { // Filter by object dynamic status if (!is_object_to_show(showing_dynamic_status, object)) continue; - const auto line_width = get_line_width(); // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.pose_with_covariance.pose.position, - object.kinematics.pose_with_covariance.pose.orientation, object.classification, line_width); + object.kinematics.pose_with_covariance.pose.orientation, object.classification, + get_line_width(), + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; @@ -93,15 +95,27 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(id_marker_ptr); } - // Get marker for pose with covariance + // Get marker for pose covariance auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.pose_with_covariance); + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - add_marker(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + // Get marker for existence probability geometry_msgs::msg::Point existence_probability_position; existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5; @@ -148,13 +162,46 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, line_width); + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; twist_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + + // Get marker for twist covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.5); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } } } diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 93f77c651869d..67279d0ae2b7f 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -5,8 +5,6 @@ 0.0.0 The component_interface_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml index ebf0684d0066c..cff1829473e86 100644 --- a/common/component_interface_tools/package.xml +++ b/common/component_interface_tools/package.xml @@ -5,8 +5,6 @@ 0.1.0 The component_interface_tools package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index dc82fda3f5c64..7a6a6d12880ad 100755 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The component_interface_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp index 3e982ac2ccf4d..532606248cd1f 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -34,6 +34,7 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { lanelet::projection::MGRSProjector projector{}; + projector.setMGRSCode(projector_info.mgrs_grid); return std::make_unique(projector); } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index d81f75815ecc6..b9136bc4002e3 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -952,7 +952,10 @@ calcArcLength>( template std::vector calcCurvature(const T & points) { - std::vector curvature_vec(points.size()); + std::vector curvature_vec(points.size(), 0.0); + if (points.size() < 3) { + return curvature_vec; + } for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index b5e770a0ea624..e796bbd0d20f7 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -5,8 +5,6 @@ 0.0.0 The path_distance_calculator package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake diff --git a/common/tier4_api_utils/package.xml b/common/tier4_api_utils/package.xml index 091f512480150..d05b810caf904 100644 --- a/common/tier4_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_api_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_datetime_rviz_plugin/package.xml b/common/tier4_datetime_rviz_plugin/package.xml index 6dc0c09b32a3b..82e1e82c61ba2 100644 --- a/common/tier4_datetime_rviz_plugin/package.xml +++ b/common/tier4_datetime_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_datetime_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt index 05cf35b93ef9f..02e65ad759ed6 100644 --- a/common/tier4_debug_rviz_plugin/CMakeLists.txt +++ b/common/tier4_debug_rviz_plugin/CMakeLists.txt @@ -12,7 +12,9 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(tier4_debug_rviz_plugin SHARED include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp + include/tier4_debug_rviz_plugin/string_stamped.hpp src/float32_multi_array_stamped_pie_chart.cpp + src/string_stamped.cpp src/jsk_overlay_utils.cpp ) diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png differ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp new file mode 100644 index 0000000000000..0960875d7eee2 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp @@ -0,0 +1,107 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// 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 {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 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.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace rviz_plugins +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage(const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml index c1158465e1cf1..e18900afc8d84 100644 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml @@ -4,4 +4,9 @@ base_class_type="rviz_common::Display"> Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped + + Display drivable area of tier4_debug_msgs::msg::StringStamped + diff --git a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp new file mode 100644 index 0000000000000..538dc0cbe7069 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp @@ -0,0 +1,198 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// 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 {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 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.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "tier4_debug_rviz_plugin/string_stamped.hpp" + +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + last_msg_ptr_->data.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index e103db19f1d43..f2482b2d45a3b 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.1.0 The tier4_localization_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Takamasa Horibe Apache License 2.0 diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 186481e92cdc4..2e83c5ab847c4 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -221,6 +221,7 @@ class MPC double m_min_prediction_length = 5.0; // Minimum prediction distance. + rclcpp::Publisher::SharedPtr m_debug_frenet_predicted_trajectory_pub; /** * @brief Get variables for MPC calculation. * @param trajectory The reference trajectory. @@ -333,6 +334,19 @@ class MPC const MPCMatrix & m, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered, const float current_steer, const double predict_dt) const; + /** + * @brief calculate predicted trajectory + * @param mpc_matrix The MPC matrix used in the mpc problem. + * @param x0 initial state used in the mpc problem. + * @param Uex optimized input. + * @param mpc_resampled_ref_traj reference trajectory resampled in the mpc time-step + * @param dt delta time used in the mpc problem. + * @return predicted path + */ + Trajectory calculatePredictedTrajectory( + const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & mpc_resampled_ref_traj, const double dt) const; + /** * @brief Check if the MPC matrix has any invalid values. * @param m The MPC matrix to check. @@ -352,18 +366,6 @@ class MPC : m_param.nominal_weight; } - /** - * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result. - * @param mpc_resampled_reference_trajectory The resampled reference trajectory. - * @param mpc_matrix The MPC matrix used for optimization. - * @param x0_delayed The delayed initial state vector. - * @param Uex The optimized input vector. - * @return The predicted trajectory. - */ - Trajectory calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const; - /** * @brief Generate diagnostic data for debugging purposes. * @param reference_trajectory The reference trajectory. @@ -424,8 +426,10 @@ class MPC double ego_nearest_dist_threshold = 3.0; // Threshold for nearest index search based on distance. double ego_nearest_yaw_threshold = M_PI_2; // Threshold for nearest index search based on yaw. + bool m_debug_publish_predicted_trajectory = false; // Flag to publish debug predicted trajectory + //!< Constructor. - MPC() = default; + explicit MPC(rclcpp::Node & node); /** * @brief Calculate control command using the MPC algorithm. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 441101abfc243..92b01247105c6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -91,7 +91,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; - MPC m_mpc; // MPC object for trajectory following. + std::unique_ptr m_mpc; // MPC object for trajectory following. // Check is mpc output converged bool m_is_mpc_history_filled{false}; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index f77ef72608edd..7e1c7ebdf1348 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -85,6 +85,12 @@ class MPCTrajectory */ size_t size() const; + /** + * @brief get trajectory point at index i + * @return trajectory point at index i + */ + MPCTrajectoryPoint at(const size_t i) const; + /** * @return true if the components sizes are all 0 or are inconsistent */ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 036e6a32a9b83..9062ff1ea34e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -92,6 +92,13 @@ Trajectory convertToAutowareTrajectory(const MPCTrajectory & input); */ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector & arc_length); +/** + * @brief calculate the arc length of the given trajectory + * @param [in] trajectory trajectory for which to calculate the arc length + * @return total arc length + */ +double calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory); + /** * @brief resample the given trajectory with the given fixed interval * @param [in] input trajectory to resample @@ -194,6 +201,14 @@ double calcStopDistance(const Trajectory & current_trajectory, const int origin) void extendTrajectoryInYawDirection( const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj); +/** + * @brief clip trajectory size by length + * @param [in] trajectory original trajectory + * @param [in] length clip length + * @return clipped trajectory + */ +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length); + /** * @brief Updates the value of a parameter with the given name. * @tparam T The type of the parameter value. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 29cab73826f31..488a3c7ab8be2 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -103,6 +103,16 @@ class DynamicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "dynamics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_lf; //!< @brief length from center of mass to front wheel [m] double m_lr; //!< @brief length from center of mass to rear wheel [m] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index fb113d5e30df7..e2f66e95ab0e3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -91,6 +91,16 @@ class KinematicsBicycleModel : public VehicleModelInterface std::string modelName() override { return "kinematics"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] double m_steer_tau; //!< @brief steering time constant for 1d-model [s] diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index 93d84caa89992..b503ad8eb1d13 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -87,6 +87,16 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface std::string modelName() override { return "kinematics_no_delay"; }; + MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + + MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const override; + private: double m_steer_lim; //!< @brief steering angle limit [rad] }; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 808ec1b7def2c..68806ff5a00dc 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -15,6 +15,8 @@ #ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ #define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#include "mpc_lateral_controller/mpc_trajectory.hpp" + #include #include @@ -55,25 +57,25 @@ class VehicleModelInterface * @brief get state x dimension * @return state dimension */ - int getDimX(); + int getDimX() const; /** * @brief get input u dimension * @return input dimension */ - int getDimU(); + int getDimU() const; /** * @brief get output y dimension * @return output dimension */ - int getDimY(); + int getDimY() const; /** * @brief get wheelbase of the vehicle * @return wheelbase value [m] */ - double getWheelbase(); + double getWheelbase() const; /** * @brief set velocity @@ -109,6 +111,42 @@ class VehicleModelInterface * @brief returns model name e.g. kinematics, dynamics */ virtual std::string modelName() = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in world + * coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; + + /** + * @brief Calculate the predicted trajectory for the ego vehicle based on the MPC result in Frenet + * Coordinate + * @param a_d The MPC A matrix used for optimization. + * @param b_d The MPC B matrix used for optimization. + * @param c_d The MPC C matrix used for optimization. + * @param w_d The MPC W matrix used for optimization. + * @param x0 initial state vector. + * @param Uex The optimized input vector. + * @param reference_trajectory The resampled reference trajectory. + * @param dt delta time used in the optimization + * @return The predicted trajectory. + */ + virtual MPCTrajectory calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, const Eigen::MatrixXd & c_d, + const Eigen::MatrixXd & w_d, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 59d3f759c9c2f..182458d532c8a 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -83,3 +83,5 @@ mass_rr: 600.0 cf: 155494.663 cr: 155494.663 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 153c2ae61d076..8fa95f4b3b87b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -17,6 +17,7 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" +#include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -28,6 +29,12 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::rad2deg; +MPC::MPC(rclcpp::Node & node) +{ + m_debug_frenet_predicted_trajectory_pub = node.create_publisher( + "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1)); +} + bool MPC::calculateMPC( const SteeringReport & current_steer, const Odometry & current_kinematics, AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, @@ -97,9 +104,9 @@ bool MPC::calculateMPC( m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev; m_raw_steer_cmd_prev = Uex(0); - // calculate predicted trajectory + /* calculate predicted trajectory */ predicted_trajectory = - calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex); + calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt); // prepare diagnostic message diagnostic = @@ -108,30 +115,6 @@ bool MPC::calculateMPC( return true; } -Trajectory MPC::calcPredictedTrajectory( - const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix, - const VectorXd & x0_delayed, const VectorXd & Uex) const -{ - const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex; - MPCTrajectory mpc_predicted_traj; - const auto & traj = mpc_resampled_ref_trajectory; - for (int i = 0; i < m_param.prediction_horizon; ++i) { - const int DIM_X = m_vehicle_model_ptr->getDimX(); - const double lat_error = Xex(i * DIM_X); - const double yaw_error = Xex(i * DIM_X + 1); - const double x = traj.x.at(i) - std::sin(traj.yaw.at(i)) * lat_error; - const double y = traj.y.at(i) + std::cos(traj.yaw.at(i)) * lat_error; - const double z = traj.z.at(i); - const double yaw = traj.yaw.at(i) + yaw_error; - const double vx = traj.vx.at(i); - const double k = traj.k.at(i); - const double smooth_k = traj.smooth_k.at(i); - const double relative_time = traj.relative_time.at(i); - mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time); - } - return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj); -} - Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, @@ -794,6 +777,35 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( return steer_rate_limits; } +Trajectory MPC::calculatePredictedTrajectory( + const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + const auto predicted_mpc_trajectory = + m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate( + mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, + dt); + + // do not over the reference trajectory + const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory); + const auto clipped_trajectory = + MPCUtils::clipTrajectoryByLength(predicted_mpc_trajectory, predicted_length); + + const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory); + + // Publish trajectory in relative coordinate for debug purpose. + if (m_debug_publish_predicted_trajectory) { + const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate( + mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, + dt); + const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( + MPCUtils::clipTrajectoryByLength(frenet, predicted_length)); + m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped); + } + + return predicted_trajectory; +} + bool MPC::isValid(const MPCMatrix & m) const { if ( diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 3d19104112a71..7eca1481ba921 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -42,7 +42,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_double = [&](const std::string & s) { return node.declare_parameter(s); }; - m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double(); + m_mpc = std::make_unique(node); + + m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double(); auto & p_filt = m_trajectory_filtering_param; p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing"); @@ -52,10 +54,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) p_filt.traj_resample_dist = dp_double("traj_resample_dist"); p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control"); - m_mpc.m_admissible_position_error = dp_double("admissible_position_error"); - m_mpc.m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); - m_mpc.m_use_steer_prediction = dp_bool("use_steer_prediction"); - m_mpc.m_param.steer_tau = dp_double("vehicle_model_steer_tau"); + m_mpc->m_admissible_position_error = dp_double("admissible_position_error"); + m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); + m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction"); + m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau"); /* stop state parameters */ m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed"); @@ -70,7 +72,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; - m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; + m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad; // steer rate limit depending on curvature const auto steer_rate_lim_dps_list_by_curvature = @@ -78,7 +80,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto curvature_list_for_steer_rate_lim = node.declare_parameter>("curvature_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back( + m_mpc->m_steer_rate_lim_map_by_curvature.emplace_back( curvature_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad); } @@ -89,26 +91,26 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) const auto velocity_list_for_steer_rate_lim = node.declare_parameter>("velocity_list_for_steer_rate_lim"); for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) { - m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back( + m_mpc->m_steer_rate_lim_map_by_velocity.emplace_back( velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad); } /* vehicle model setup */ auto vehicle_model_ptr = - createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node); - m_mpc.setVehicleModel(vehicle_model_ptr); + createVehicleModel(wheelbase, m_mpc->m_steer_lim, m_mpc->m_param.steer_tau, node); + m_mpc->setVehicleModel(vehicle_model_ptr); /* QP solver setup */ - m_mpc.setVehicleModel(vehicle_model_ptr); + m_mpc->setVehicleModel(vehicle_model_ptr); auto qpsolver_ptr = createQPSolverInterface(node); - m_mpc.setQPSolver(qpsolver_ptr); + m_mpc->setQPSolver(qpsolver_ptr); /* delay compensation */ { const double delay_tmp = dp_double("input_delay"); - const double delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); - m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + const double delay_step = std::round(delay_tmp / m_mpc->m_ctrl_period); + m_mpc->m_param.input_delay = delay_step * m_mpc->m_ctrl_period; + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } /* steering offset compensation */ @@ -120,7 +122,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) { const double steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz"); const double error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz"); - m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + m_mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } // ego nearest index search @@ -129,8 +131,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) }; m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold"); m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold"); - m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; - m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + m_mpc->ego_nearest_dist_threshold = m_ego_nearest_dist_threshold; + m_mpc->ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold; + + m_mpc->m_debug_publish_predicted_trajectory = dp_bool("debug_publish_predicted_trajectory"); m_pub_predicted_traj = node.create_publisher("~/output/predicted_trajectory", 1); m_pub_debug_values = @@ -144,10 +148,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_set_param_res = node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1)); - m_mpc.initializeSteeringPredictor(); + m_mpc->initializeSteeringPredictor(); - m_mpc.setLogger(logger_); - m_mpc.setClock(clock_); + m_mpc->setLogger(logger_); + m_mpc->setClock(clock_); } MpcLateralController::~MpcLateralController() @@ -244,7 +248,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_ctrl_cmd_prev_initialized = true; } - const bool is_mpc_solved = m_mpc.calculateMPC( + const bool is_mpc_solved = m_mpc->calculateMPC( m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values); // reset previous MPC result @@ -253,7 +257,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( // After the recovery, the previous value of the optimization may deviate greatly from // the actual steer angle, and it may make the optimization result unstable. if (!is_mpc_solved) { - m_mpc.resetPrevResult(m_current_steering); + m_mpc->resetPrevResult(m_current_steering); } else { setSteeringToHistory(ctrl_cmd); } @@ -284,11 +288,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( if (isStoppedState()) { // Reset input buffer - for (auto & value : m_mpc.m_input_buffer) { + for (auto & value : m_mpc->m_input_buffer) { value = m_ctrl_cmd_prev.steering_tire_angle; } // Use previous command value as previous raw steer command - m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; + m_mpc->m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; return createLateralOutput(m_ctrl_cmd_prev, false); } @@ -323,15 +327,15 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; - if (!m_mpc.hasVehicleModel()) { + if (!m_mpc->hasVehicleModel()) { info_throttle("MPC does not have a vehicle model"); return false; } - if (!m_mpc.hasQPSolver()) { + if (!m_mpc->hasQPSolver()) { info_throttle("MPC does not have a QP solver"); return false; } - if (m_mpc.m_reference_trajectory.empty()) { + if (m_mpc->m_reference_trajectory.empty()) { info_throttle("trajectory size is zero."); return false; } @@ -354,7 +358,7 @@ void MpcLateralController::setTrajectory( return; } - m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); + m_mpc->setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); @@ -504,12 +508,12 @@ bool MpcLateralController::isMpcConverged() void MpcLateralController::declareMPCparameters(rclcpp::Node & node) { - m_mpc.m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); + m_mpc->m_param.prediction_horizon = node.declare_parameter("mpc_prediction_horizon"); + m_mpc->m_param.prediction_dt = node.declare_parameter("mpc_prediction_dt"); const auto dp = [&](const auto & param) { return node.declare_parameter(param); }; - auto & nw = m_mpc.m_param.nominal_weight; + auto & nw = m_mpc->m_param.nominal_weight; nw.lat_error = dp("mpc_weight_lat_error"); nw.heading_error = dp("mpc_weight_heading_error"); nw.heading_error_squared_vel = dp("mpc_weight_heading_error_squared_vel"); @@ -521,7 +525,7 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) nw.terminal_lat_error = dp("mpc_weight_terminal_lat_error"); nw.terminal_heading_error = dp("mpc_weight_terminal_heading_error"); - auto & lcw = m_mpc.m_param.low_curvature_weight; + auto & lcw = m_mpc->m_param.low_curvature_weight; lcw.lat_error = dp("mpc_low_curvature_weight_lat_error"); lcw.heading_error = dp("mpc_low_curvature_weight_heading_error"); lcw.heading_error_squared_vel = dp("mpc_low_curvature_weight_heading_error_squared_vel"); @@ -530,12 +534,12 @@ void MpcLateralController::declareMPCparameters(rclcpp::Node & node) lcw.lat_jerk = dp("mpc_low_curvature_weight_lat_jerk"); lcw.steer_rate = dp("mpc_low_curvature_weight_steer_rate"); lcw.steer_acc = dp("mpc_low_curvature_weight_steer_acc"); - m_mpc.m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); + m_mpc->m_param.low_curvature_thresh_curvature = dp("mpc_low_curvature_thresh_curvature"); - m_mpc.m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); - m_mpc.m_param.acceleration_limit = dp("mpc_acceleration_limit"); - m_mpc.m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); - m_mpc.m_param.min_prediction_length = dp("mpc_min_prediction_length"); + m_mpc->m_param.zero_ff_steer_deg = dp("mpc_zero_ff_steer_deg"); + m_mpc->m_param.acceleration_limit = dp("mpc_acceleration_limit"); + m_mpc->m_param.velocity_time_constant = dp("mpc_velocity_time_constant"); + m_mpc->m_param.min_prediction_length = dp("mpc_min_prediction_length"); } rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( @@ -546,7 +550,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( result.reason = "success"; // strong exception safety wrt MPCParam - MPCParam param = m_mpc.m_param; + MPCParam param = m_mpc->m_param; auto & nw = param.nominal_weight; auto & lcw = param.low_curvature_weight; @@ -587,15 +591,15 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( // initialize input buffer update_param(parameters, "input_delay", param.input_delay); - const double delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); - const double delay = delay_step * m_mpc.m_ctrl_period; + const double delay_step = std::round(param.input_delay / m_mpc->m_ctrl_period); + const double delay = delay_step * m_mpc->m_ctrl_period; if (param.input_delay != delay) { param.input_delay = delay; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); + m_mpc->m_input_buffer = std::deque(static_cast(delay_step), 0.0); } // transaction succeeds, now assign values - m_mpc.m_param = param; + m_mpc->m_param = param; } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/mpc_lateral_controller/src/mpc_trajectory.cpp index 3c799722494b9..b7fed942375e8 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/mpc_lateral_controller/src/mpc_trajectory.cpp @@ -58,6 +58,22 @@ MPCTrajectoryPoint MPCTrajectory::back() return p; } +MPCTrajectoryPoint MPCTrajectory::at(const size_t i) const +{ + MPCTrajectoryPoint p; + + p.x = x.at(i); + p.y = y.at(i); + p.z = z.at(i); + p.yaw = yaw.at(i); + p.vx = vx.at(i); + p.k = k.at(i); + p.smooth_k = smooth_k.at(i); + p.relative_time = relative_time.at(i); + + return p; +} + void MPCTrajectory::clear() { x.clear(); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index d3fc5fba0b014..01a81d9015b47 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -89,6 +89,15 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, const double ego_offset_to_segment) @@ -458,5 +467,22 @@ void extendTrajectoryInYawDirection( } } +MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length) +{ + MPCTrajectory clipped_trajectory; + clipped_trajectory.push_back(trajectory.at(0)); + + double current_length = 0.0; + for (size_t i = 1; i < trajectory.size(); ++i) { + current_length += calcDistance3d(trajectory, i, i - 1); + if (current_length > length) { + break; + } + clipped_trajectory.push_back(trajectory.at(i)); + } + + return clipped_trajectory; +} + } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 576907e79da4b..2467b1f0c2311 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -86,4 +86,48 @@ void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase); u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature; } + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + RCLCPP_ERROR( + rclcpp::get_logger("control.trajectory_follower.lateral_controller"), + "Predicted trajectory calculation in world coordinate is not supported in dynamic model. " + "Calculate in the Frenet coordinate instead."); + return calculatePredictedTrajectoryInFrenetCoordinate( + a_d, b_d, c_d, w_d, x0, Uex, reference_trajectory, dt); +} + +MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // state = [e, de, th, dth] + // e : lateral error + // de : derivative of lateral error + // th : heading angle error + // dth : derivative of heading angle error + // steer : steering angle (input) + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 2); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 35bef667125c5..cd52dd4f79314 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -68,4 +68,85 @@ void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref) { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err, steer] + // World coordinate x = [x, y, yaw, steer] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(4); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + state(3, 0) = x0(2); // steering + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = state_w(3); + const auto desired_steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + dstate(3) = -(steer - desired_steer) / m_steer_tau; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err, steer] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 1536beba9cd00..8f4510510aca9 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -59,4 +59,82 @@ void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ { u_ref(0, 0) = std::atan(m_wheelbase * m_curvature); } + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInWorldCoordinate( + [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, const double dt) const +{ + // Calculate predicted state in world coordinate since there is modeling errors in Frenet + // Relative coordinate x = [lat_err, yaw_err] + // World coordinate x = [x, y, yaw] + + const auto & t = reference_trajectory; + + // create initial state in the world coordinate + Eigen::VectorXd state_w = [&]() { + Eigen::VectorXd state = Eigen::VectorXd::Zero(3); + const auto lateral_error_0 = x0(0); + const auto yaw_error_0 = x0(1); + state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw + return state; + }(); + + // update state in the world coordinate + const auto updateState = [&]( + const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, + const double dt, const double velocity) { + const auto yaw = state_w(2); + const auto steer = input(0); + + Eigen::VectorXd dstate = Eigen::VectorXd::Zero(3); + dstate(0) = velocity * std::cos(yaw); + dstate(1) = velocity * std::sin(yaw); + dstate(2) = velocity * std::tan(steer) / m_wheelbase; + + // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation + // in Eigen. + const Eigen::VectorXd next_state = state_w + dstate * dt; + return next_state; + }; + + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_U = getDimU(); + for (size_t i = 0; i < t.size(); ++i) { + state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + mpc_predicted_trajectory.push_back( + state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), + t.relative_time.at(i)); + } + + return mpc_predicted_trajectory; +} + +MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInFrenetCoordinate( + const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d, + [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d, + const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex, + const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const +{ + // Relative coordinate x = [lat_err, yaw_err] + + Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d; + MPCTrajectory mpc_predicted_trajectory; + const auto DIM_X = getDimX(); + const auto & t = reference_trajectory; + + for (size_t i = 0; i < reference_trajectory.size(); ++i) { + const auto lateral_error = Xex(i * DIM_X); // model dependent + const auto yaw_error = Xex(i * DIM_X + 1); // model dependent + const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error; + const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error; + const auto yaw = t.yaw.at(i) + yaw_error; + mpc_predicted_trajectory.push_back( + x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i)); + } + return mpc_predicted_trajectory; +} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index 97e3f2742f546..f54a5e325bead 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -20,19 +20,19 @@ VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, do : m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase) { } -int VehicleModelInterface::getDimX() +int VehicleModelInterface::getDimX() const { return m_dim_x; } -int VehicleModelInterface::getDimU() +int VehicleModelInterface::getDimU() const { return m_dim_u; } -int VehicleModelInterface::getDimY() +int VehicleModelInterface::getDimY() const { return m_dim_y; } -double VehicleModelInterface::getWheelbase() +double VehicleModelInterface::getWheelbase() const { return m_wheelbase; } diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index dade035daf26c..ba145b5dd146c 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -19,6 +19,7 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "rclcpp/rclcpp.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -48,6 +49,22 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; +TrajectoryPoint makePoint(const double x, const double y, const float vx) +{ + TrajectoryPoint p; + p.pose.position.x = x; + p.pose.position.y = y; + p.longitudinal_velocity_mps = vx; + return p; +} + +nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) +{ + nav_msgs::msg::Odometry odometry; + odometry.pose.pose = pose; + odometry.twist.twist.linear.x = velocity; + return odometry; +} class MPCTest : public ::testing::Test { protected: @@ -86,16 +103,7 @@ class MPCTest : public ::testing::Test TrajectoryFilteringParam trajectory_param; - TrajectoryPoint makePoint(const double x, const double y, const float vx) - { - TrajectoryPoint p; - p.pose.position.x = x; - p.pose.position.y = y; - p.longitudinal_velocity_mps = vx; - return p; - } - - void SetUp() override + void initParam() { param.prediction_horizon = 50; param.prediction_dt = 0.1; @@ -168,259 +176,268 @@ class MPCTest : public ::testing::Test mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); } - nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) + void SetUp() override { - nav_msgs::msg::Odometry odometry; - odometry.pose.pose = pose; - odometry.twist.twist.linear.x = velocity; - return odometry; + rclcpp::init(0, nullptr); + initParam(); } + + void TearDown() override { rclcpp::shutdown(); } }; // class MPCTest /* cppcheck-suppress syntaxError */ TEST_F(MPCTest, InitializeAndCalculate) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, InitializeAndCalculateRightTurn) { - MPC mpc; - EXPECT_FALSE(mpc.hasVehicleModel()); - EXPECT_FALSE(mpc.hasQPSolver()); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + EXPECT_FALSE(mpc->hasVehicleModel()); + EXPECT_FALSE(mpc->hasQPSolver()); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, OsqpCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(logger); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); const auto current_kinematics = makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); + mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Init filters - mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); + mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_LT(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_LT(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, DynamicCalculate) { - MPC mpc; - initializeMPC(mpc); + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); + initializeMPC(*mpc); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); - mpc.setVehicleModel(vehicle_model_ptr); - ASSERT_TRUE(mpc.hasVehicleModel()); + mpc->setVehicleModel(vehicle_model_ptr); + ASSERT_TRUE(mpc->hasVehicleModel()); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); - ASSERT_TRUE(mpc.hasQPSolver()); + mpc->setQPSolver(qpsolver_ptr); + ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); } TEST_F(MPCTest, MultiSolveWithBuffer) { - MPC mpc; + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); - mpc.m_input_buffer = {0.0, 0.0, 0.0}; + mpc->m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); - ASSERT_TRUE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); + ASSERT_TRUE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); EXPECT_EQ(ctrl_cmd.steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd.steering_tire_rotation_rate, 0.0f); - EXPECT_EQ(mpc.m_input_buffer.size(), size_t(3)); + EXPECT_EQ(mpc->m_input_buffer.size(), size_t(3)); } TEST_F(MPCTest, FailureCases) { - MPC mpc; + auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); + auto mpc = std::make_unique(node); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); - mpc.setVehicleModel(vehicle_model_ptr); + mpc->setVehicleModel(vehicle_model_ptr); std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc.setQPSolver(qpsolver_ptr); + mpc->setQPSolver(qpsolver_ptr); // Init parameters and reference trajectory - initializeMPC(mpc); + initializeMPC(*mpc); // Calculate MPC with a pose too far from the trajectory Pose pose_far; @@ -430,10 +447,10 @@ TEST_F(MPCTest, FailureCases) Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); - EXPECT_FALSE(mpc.calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); + EXPECT_FALSE(mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag)); // Calculate MPC with a fast velocity to make the prediction go further than the reference path - EXPECT_FALSE(mpc.calculateMPC( + EXPECT_FALSE(mpc->calculateMPC( neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, pred_traj, diag)); } } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/trajectory_follower_node/param/lateral/mpc.param.yaml index 4222082d40deb..9998b6aadf656 100644 --- a/control/trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/trajectory_follower_node/param/lateral/mpc.param.yaml @@ -74,3 +74,5 @@ update_steer_threshold: 0.035 average_num: 1000 steering_offset_limit: 0.02 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 4d41ad0ac4a83..c55f1c8038e83 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_autoware_api_launch package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 3fba4bd773e68..f4d6679291849 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -15,14 +15,15 @@ - - + - + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml index e4b04d3d4a32e..bee300c587816 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml @@ -2,13 +2,16 @@ - - + + + + + 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 7733b4e3117a1..02c6da20e17da 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 @@ -23,7 +23,9 @@ - + + + @@ -142,7 +144,9 @@ - + + + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py index 1a34429f438ed..22a45fe7b8530 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ b/launch/tier4_localization_launch/launch/util/util.launch.py @@ -15,8 +15,6 @@ import launch from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction -from launch.conditions import LaunchConfigurationNotEquals -from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -71,16 +69,9 @@ def load_composable_node_param(param_path): random_downsample_component, ] - target_container = ( - "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container" - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("pointcloud_container_name") - ) - load_composable_nodes = LoadComposableNodes( - condition=LaunchConfigurationNotEquals(target_container, ""), composable_node_descriptions=composable_nodes, - target_container=target_container, + target_container=LaunchConfiguration("lidar_container_name"), ) return [load_composable_nodes] @@ -115,11 +106,10 @@ def add_launch_arg(name: str, default_value=None, description=None): "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("use_pointcloud_container", "True", "use pointcloud container") add_launch_arg( - "pointcloud_container_name", - "/pointcloud_container", - "container name", + "lidar_container_name", + "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container", + "container name of main lidar used for localization", ) add_launch_arg( diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index c4de9c04dcaf2..9b4f727c9ce52 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -8,6 +8,9 @@ Koji Minoda Kento Yabuuchi Ryu Yamamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda Apache License 2.0 Yamato Ando 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/camera_lidar_fusion_based_detection.launch.xml index 227aac50d6d90..4cb648852a03c 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/camera_lidar_fusion_based_detection.launch.xml @@ -6,15 +6,12 @@ - + - - - @@ -65,7 +62,7 @@ - + @@ -113,6 +110,7 @@ + @@ -167,9 +165,7 @@ - - - + @@ -271,6 +267,8 @@ + + @@ -305,6 +303,7 @@ + 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 index 65caad54876ff..ba86bc1e334ff 100644 --- 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 @@ -6,7 +6,7 @@ - + @@ -91,7 +91,7 @@ - + @@ -139,6 +139,7 @@ + @@ -193,9 +194,7 @@ - - - + @@ -297,6 +296,8 @@ + + @@ -344,6 +345,7 @@ + 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 0649d8c5d5116..885fa80ed7004 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 @@ -8,7 +8,7 @@ - + @@ -34,11 +34,6 @@ - - - - - @@ -64,15 +59,15 @@ - - - - + + + + @@ -101,7 +96,7 @@ - + @@ -116,7 +111,7 @@ - + @@ -131,7 +126,7 @@ - + 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/lidar_based_detection.launch.xml index 2c8b7d6581c53..04c4264b70e5f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -6,7 +6,7 @@ - + @@ -25,7 +25,7 @@ - + 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 index ed37f6270c913..adedc2312677f 100644 --- 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 @@ -5,7 +5,7 @@ - + @@ -23,7 +23,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index 93d395ca3e466..1dcb9cfc90daf 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -141,7 +141,7 @@ def launch_setup(context, *args, **kwargs): components = [] components.extend(pipeline.create_pipeline()) individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -151,7 +151,7 @@ def launch_setup(context, *args, **kwargs): ) pointcloud_container_loader = LoadComposableNodes( composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) return [individual_container, pointcloud_container_loader] @@ -168,7 +168,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "pointcloud_map_filter_container") add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index fecdd29bcb7e9..66a0bb4d0fb63 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -474,13 +474,7 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, def launch_setup(context, *args, **kwargs): pipeline = GroundSegmentationPipeline(context) - glog_component = ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - - components = [glog_component] + components = [] components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"), @@ -511,7 +505,7 @@ def launch_setup(context, *args, **kwargs): ) ) individual_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -521,7 +515,7 @@ def launch_setup(context, *args, **kwargs): ) pointcloud_container_loader = LoadComposableNodes( composable_node_descriptions=components, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) return [individual_container, pointcloud_container_loader] @@ -537,7 +531,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "perception_pipeline_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "ground_segmentation_container") add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index 281a52a85af71..244e0940acb70 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -7,7 +7,7 @@ - + @@ -24,7 +24,7 @@ - + @@ -40,7 +40,7 @@ - + @@ -56,7 +56,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 1b5753fb4d5c4..528038c5158b2 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -10,6 +10,9 @@ + + + @@ -31,6 +34,9 @@ + + + @@ -99,11 +105,6 @@ description="options: `ped_traffic_light_classifier_mobilenetv2_batch_*` or `ped_traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6" /> - - - - - @@ -126,7 +127,7 @@ - + @@ -141,7 +142,7 @@ - + @@ -177,6 +178,9 @@ + + + @@ -189,10 +193,7 @@ - - - - + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index bf81125286256..e02ba883d5115 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -7,6 +7,8 @@ + + @@ -21,7 +23,9 @@ - + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 73e466afc0bda..bae084f80b51e 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -1,4 +1,6 @@ + + @@ -8,6 +10,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 1c7643359c496..a297697234ef2 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 @@ -4,6 +4,7 @@ + @@ -195,6 +196,7 @@ + @@ -235,6 +237,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 66c90ef2ff833..e673d28804480 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -1,4 +1,5 @@ + @@ -50,7 +51,9 @@ - + + + diff --git a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index 0493e1ae0bbcf..0e1d22bfd1827 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -6,6 +6,7 @@ + @@ -21,6 +22,7 @@ + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 87cc510c5fe14..02f14a86cd7df 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -83,6 +83,7 @@ The parameters are set in `launch/ekf_localizer.launch` . | 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 | diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 1db45db411b0c..9de7f56f5c006 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -4,6 +4,7 @@ enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 + publish_tf: true extend_state_step: 50 # for Pose measurement diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index ba18b7dadd599..6925e8b63c66b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -199,7 +199,7 @@ class EKFLocalizer : public rclcpp::Node /** * @brief update predict frequency */ - void updatePredictFrequency(); + void updatePredictFrequency(const rclcpp::Time & current_time); /** * @brief get transform from frame_id @@ -219,7 +219,7 @@ class EKFLocalizer : public rclcpp::Node /** * @brief publish diagnostics message */ - void publishDiagnostics(); + void publishDiagnostics(const rclcpp::Time & current_time); /** * @brief update simple1DFilter diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index e0e34f382e3dc..5139f900a339e 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -28,6 +28,7 @@ class HyperParameters ekf_rate(node->declare_parameter("predict_frequency", 50.0)), 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"))), @@ -60,6 +61,7 @@ class HyperParameters const double ekf_rate; const double ekf_dt; const double tf_rate_; + const bool publish_tf_; const bool enable_yaw_bias_estimation; const int extend_state_step; const std::string pose_frame_id; diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 4101bae88b6e2..e9d756e547f26 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -9,6 +9,11 @@ Yamato Ando Takeshi Ishita Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Takamasa Horibe diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 0ab88202e0c0d..f77481d45a534 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -60,9 +60,11 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), std::bind(&EKFLocalizer::timerCallback, this)); - timer_tf_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + if (params_.publish_tf_) { + timer_tf_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), + std::bind(&EKFLocalizer::timerTFCallback, this)); + } pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = @@ -102,14 +104,14 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti /* * updatePredictFrequency */ -void EKFLocalizer::updatePredictFrequency() +void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) { if (last_predict_time_) { - if (get_clock()->now() < *last_predict_time_) { + if (current_time < *last_predict_time_) { warning_->warn("Detected jump back in time"); } else { /* Measure dt */ - ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds(); + ekf_dt_ = (current_time - *last_predict_time_).seconds(); DEBUG_INFO( get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_); @@ -131,7 +133,7 @@ void EKFLocalizer::updatePredictFrequency() proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0); } } - last_predict_time_ = std::make_shared(get_clock()->now()); + last_predict_time_ = std::make_shared(current_time); } /* @@ -139,17 +141,19 @@ void EKFLocalizer::updatePredictFrequency() */ void EKFLocalizer::timerCallback() { + const rclcpp::Time current_time = this->now(); + if (!is_activated_) { warning_->warnThrottle( "The node is not activated. Provide initial pose to pose_initializer", 2000); - publishDiagnostics(); + publishDiagnostics(current_time); return; } DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ - updatePredictFrequency(); + updatePredictFrequency(current_time); /* predict model in EKF */ stop_watch_.tic(); @@ -173,7 +177,7 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = this->now(); + const auto t_curr = current_time; const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); @@ -208,7 +212,7 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = this->now(); + const auto t_curr = current_time; const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); @@ -226,15 +230,15 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, true); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->getCurrentTwist(this->now()); + ekf_module_->getCurrentTwist(current_time); /* publish ekf result */ publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publishDiagnostics(); + publishDiagnostics(current_time); } /* @@ -254,10 +258,12 @@ void EKFLocalizer::timerTFCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); + const rclcpp::Time current_time = this->now(); + geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tier4_autoware_utils::pose2transform( - ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false), "base_link"); - transform_stamped.header.stamp = this->now(); + ekf_module_->getCurrentPose(current_time, z, roll, pitch, false), "base_link"); + transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); } @@ -338,15 +344,13 @@ void EKFLocalizer::publishEstimateResult( const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist) { - rclcpp::Time current_time = this->now(); - /* publish latest pose */ pub_pose_->publish(current_ekf_pose); pub_biased_pose_->publish(current_biased_ekf_pose); /* publish latest pose with covariance */ geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; - pose_cov.header.stamp = current_time; + pose_cov.header.stamp = current_ekf_pose.header.stamp; pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); @@ -361,7 +365,7 @@ void EKFLocalizer::publishEstimateResult( /* publish latest twist with covariance */ geometry_msgs::msg::TwistWithCovarianceStamped twist_cov; - twist_cov.header.stamp = current_time; + twist_cov.header.stamp = current_ekf_twist.header.stamp; twist_cov.header.frame_id = current_ekf_twist.header.frame_id; twist_cov.twist.twist = current_ekf_twist.twist; twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); @@ -369,13 +373,13 @@ void EKFLocalizer::publishEstimateResult( /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; - yawb.stamp = current_time; + yawb.stamp = current_ekf_twist.header.stamp; yawb.data = ekf_module_->getYawBias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ nav_msgs::msg::Odometry odometry; - odometry.header.stamp = current_time; + odometry.header.stamp = current_ekf_pose.header.stamp; odometry.header.frame_id = current_ekf_pose.header.frame_id; odometry.child_frame_id = "base_link"; odometry.pose = pose_cov.pose; @@ -383,7 +387,7 @@ void EKFLocalizer::publishEstimateResult( pub_odom_->publish(odometry); } -void EKFLocalizer::publishDiagnostics() +void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) { std::vector diag_status_array; @@ -419,7 +423,7 @@ void EKFLocalizer::publishDiagnostics() diag_merged_status.hardware_id = this->get_name(); diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = this->now(); + diag_msg.header.stamp = current_time; diag_msg.status.push_back(diag_merged_status); pub_diag_->publish(diag_msg); } diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index d424ff14b9c99..c0e2db59deb64 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -6,6 +6,12 @@ The geo_pose_projector package Yamato Ando Koji Minoda + Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index ad4a865a2014a..a0a982ddedc16 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -6,6 +6,11 @@ The gyro_odometer package as a ROS 2 node Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 857ec4f16f051..072479cc7aaf5 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Yamato Ando Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 ament_cmake diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index 7e78ed713dddc..c2b0751d91f9a 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -44,6 +44,8 @@ class LandmarkManager const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype); + [[nodiscard]] std::vector get_landmarks() const; + [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const; [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose( diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml index 2e0e252386d1f..be19de9334e84 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Yamato Ando Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 ament_cmake diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 7ddd66efea0a6..57bfcde461af6 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -83,6 +83,22 @@ void LandmarkManager::parse_landmarks( } } +std::vector LandmarkManager::get_landmarks() const +{ + std::vector landmarks; + + landmark_manager::Landmark landmark; + for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) { + for (const auto & pose : landmark_poses) { + landmark.id = landmark_id_str; + landmark.pose = pose; + landmarks.push_back(landmark); + } + } + + return landmarks; +} + visualization_msgs::msg::MarkerArray LandmarkManager::get_landmarks_as_marker_array_msg() const { int32_t id = 0; diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 266ae6c848834..a1b352d911a0d 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -7,6 +7,11 @@ Yamato Ando Koji Minoda Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Taichi Higashide diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml index 20f9b160212b5..fa8da2aa1231a 100644 --- a/localization/localization_util/package.xml +++ b/localization/localization_util/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Shintaro Sakoda Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 Yamato Ando 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 e9aa265c78f34..71895c59ec37d 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 @@ -127,6 +127,8 @@ class NDTScanMatcher : public rclcpp::Node const double & transform_probability, const double & nearest_voxel_transformation_likelihood); static int count_oscillation(const std::vector & result_pose_msg_array); + std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 891d172aaf8fd..cbcb0a9f74bc4 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -2,27 +2,27 @@ - - - + + + - + - - + + + - - + diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index dcfdd77cb5a18..0913ee04174f5 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -8,6 +8,10 @@ Kento Yabuuchi Koji Minoda Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando 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 0f6fdbc15db26..942df03410165 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -486,7 +486,15 @@ void NDTScanMatcher::callback_sensor_points( } // covariance estimation - std::array ndt_covariance = output_pose_covariance_; + const Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond( + result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y, + result_pose_msg.orientation.z); + const Eigen::Matrix3d map_to_base_link_rotation = + map_to_base_link_quat.normalized().toRotationMatrix(); + + std::array ndt_covariance = + rotate_covariance(output_pose_covariance_, map_to_base_link_rotation); + if (is_converged && use_cov_estimation_) { const auto estimated_covariance = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); @@ -766,6 +774,28 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } +std::array NDTScanMatcher::rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) const +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 83bce5a718334..16c49bb51c218 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -6,6 +6,11 @@ The pose2twist package Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index 1a1a8e48b7e3b..85c9c26bd6c8c 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -7,6 +7,11 @@ Yamato Ando Takagi, Isamu Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Yamato Ando Takagi, Isamu diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index 7774407a7990d..bf57e5589b747 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -10,6 +10,7 @@ Taiki Yamada Ryu Yamamoto Shintaro Sakoda + NGUYEN Viet Anh Apache License 2.0 Shintaro Sakoda diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index b17809aaa9948..83eaf1b9fa821 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -7,6 +7,11 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml index b699d5c69e610..96d2b9ecf54cc 100644 --- a/localization/tree_structured_parzen_estimator/package.xml +++ b/localization/tree_structured_parzen_estimator/package.xml @@ -8,6 +8,9 @@ Masahiro Sakamoto Shintaro Sakoda Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto Apache License 2.0 Shintaro Sakoda diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index c44cd9d144566..08dacf9185769 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -7,6 +7,11 @@ Koji Minoda Yamato Ando Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Koji Minoda diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index d7e94ebefa24b..710ec0aeb75f8 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -5,6 +5,11 @@ YabLoc common library Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index aa6fc10ee6667..a0cad16302c2b 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index ffdcea25b4b6d..209f09fdaa7ac 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -6,6 +6,11 @@ YabLoc image processing package Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml index 8c92c3c6a4303..22a5d0eded6b6 100644 --- a/localization/yabloc/yabloc_monitor/package.xml +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -7,6 +7,11 @@ Kento Yabuuchi Koji Minoda Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md index 41ab7b1b10039..d42ce4647bc95 100644 --- a/localization/yabloc/yabloc_particle_filter/README.md +++ b/localization/yabloc/yabloc_particle_filter/README.md @@ -46,6 +46,12 @@ This package contains some executable nodes related to particle filter. | `prediction_rate` | double | frequency of forecast updates, in Hz | | `cov_xx_yy` | vector\ | the covariance of initial pose | +### Services + +| Name | Type | Description | +| -------------------- | ------------------------ | ------------------------------------------------ | +| `yabloc_trigger_srv` | `std_srvs::srv::SetBool` | activation and deactivation of yabloc estimation | + ## gnss_particle_corrector ### Purpose diff --git a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp index c7794a2be9f51..e02393ee14db6 100644 --- a/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp +++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,7 @@ class Predictor : public rclcpp::Node using TwistCovStamped = geometry_msgs::msg::TwistWithCovarianceStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Marker = visualization_msgs::msg::Marker; + using SetBool = std_srvs::srv::SetBool; Predictor(); @@ -62,6 +64,7 @@ class Predictor : public rclcpp::Node const std::vector cov_xx_yy_; // Subscriber + rclcpp::Subscription::SharedPtr ekf_pose_sub_; rclcpp::Subscription::SharedPtr initialpose_sub_; rclcpp::Subscription::SharedPtr twist_cov_sub_; rclcpp::Subscription::SharedPtr particles_sub_; @@ -73,11 +76,15 @@ class Predictor : public rclcpp::Node rclcpp::Publisher::SharedPtr pose_cov_pub_; rclcpp::Publisher::SharedPtr marker_pub_; std::unique_ptr tf2_broadcaster_; + // Server + rclcpp::Service::SharedPtr yabloc_trigger_server_; // Timer callback rclcpp::TimerBase::SharedPtr timer_; float ground_height_{0}; + bool yabloc_activated_{true}; + PoseCovStamped::ConstSharedPtr latest_ekf_pose_ptr_{nullptr}; std::optional particle_array_opt_{std::nullopt}; std::optional latest_twist_opt_{std::nullopt}; @@ -92,6 +99,8 @@ class Predictor : public rclcpp::Node void on_twist_cov(const TwistCovStamped::ConstSharedPtr twist); void on_weighted_particles(const ParticleArray::ConstSharedPtr weighted_particles); void on_timer(); + void on_trigger_service( + SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response); // void initialize_particles(const PoseCovStamped & initialpose); diff --git a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml index f38264b828d34..5dd483f142174 100644 --- a/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml +++ b/localization/yabloc/yabloc_particle_filter/launch/yabloc_particle_filter.launch.xml @@ -16,6 +16,9 @@ + + + diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index aabb6009c2148..5db4aa0c29e88 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -6,6 +6,11 @@ YabLoc particle filter package Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp index b16905ba19b41..764ef88bbde18 100644 --- a/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp @@ -52,7 +52,12 @@ Predictor::Predictor() auto on_initial = std::bind(&Predictor::on_initial_pose, this, _1); auto on_twist_cov = std::bind(&Predictor::on_twist_cov, this, _1); auto on_particle = std::bind(&Predictor::on_weighted_particles, this, _1); - auto on_height = [this](std_msgs::msg::Float32 m) -> void { this->ground_height_ = m.data; }; + auto on_height = [this](std_msgs::msg::Float32::ConstSharedPtr msg) -> void { + this->ground_height_ = msg->data; + }; + auto on_ekf_pose = [this](PoseCovStamped::ConstSharedPtr msg) -> void { + this->latest_ekf_pose_ptr_ = msg; + }; initialpose_sub_ = create_subscription("~/input/initialpose", 1, on_initial); particles_sub_ = @@ -60,6 +65,7 @@ Predictor::Predictor() height_sub_ = create_subscription("~/input/height", 10, on_height); twist_cov_sub_ = create_subscription("~/input/twist_with_covariance", 10, on_twist_cov); + ekf_pose_sub_ = create_subscription("~/input/ekf_pose", 10, on_ekf_pose); // Timer callback const double prediction_rate = declare_parameter("prediction_rate"); @@ -67,12 +73,41 @@ Predictor::Predictor() timer_ = rclcpp::create_timer( this, this->get_clock(), rclcpp::Rate(prediction_rate).period(), std::move(on_timer)); + // Service server + using std::placeholders::_2; + auto on_trigger_service = std::bind(&Predictor::on_trigger_service, this, _1, _2); + yabloc_trigger_server_ = create_service("~/yabloc_trigger_srv", on_trigger_service); + // Optional modules if (declare_parameter("visualize", false)) { visualizer_ptr_ = std::make_unique(*this); } } +void Predictor::on_trigger_service( + SetBool::Request::ConstSharedPtr request, SetBool::Response::SharedPtr response) +{ + if (request->data) { + RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is activated"); + } else { + RCLCPP_INFO_STREAM(get_logger(), "yabloc particle filter is deactivated"); + } + + const bool before_activated_ = yabloc_activated_; + yabloc_activated_ = request->data; + response->success = true; + + if (yabloc_activated_ && (!before_activated_)) { + RCLCPP_INFO_STREAM(get_logger(), "restart particle filter"); + if (latest_ekf_pose_ptr_) { + on_initial_pose(latest_ekf_pose_ptr_); + } else { + yabloc_activated_ = false; + response->success = false; + } + } +} + void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose) { // Publish initial pose marker @@ -181,6 +216,10 @@ void Predictor::on_timer() { // ========================================================================== // Pre-check section + // Return if yabloc is not activated + if (!yabloc_activated_) { + return; + } // Return if particle_array is not initialized yet if (!particle_array_opt_.has_value()) { return; diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index 4e3ca1c1e61d1..e9921db50093e 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -5,6 +5,11 @@ The pose initializer Kento Yabuuchi Masahiro Sakamoto + Yamato Ando + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_ros diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 2dc864b0cb925..85258a8e54b22 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -8,6 +8,10 @@ Yamato Ando Masahiro Sakamoto Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Takagi, Isamu diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index c8fcce6f7002f..0103c7d2b74a7 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -9,6 +9,9 @@ Koji Minoda Masahiro Sakamoto Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda Apache License 2.0 diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index e14defcb47f26..a0c57759d51a6 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -215,6 +215,12 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + road_lanelets, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + crosswalk_lanelets, cl_trafficlights)); insertMarkerArray( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 76e40d4379de4..475881577bd58 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -8,6 +8,10 @@ Yamato Ando Masahiro Sakamoto Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_auto diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 6e266b0ad82f0..6f08da169309e 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -7,6 +7,10 @@ Yamato Ando Kento Yabuuchi Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 ament_cmake_auto diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index ec2f57963332f..2419cdb010799 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -124,7 +124,7 @@ See paper [2] for more details. `lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) -#### Pruning predicted paths with lateral acceleration constraint +#### Pruning predicted paths with lateral acceleration constraint (for vehicle obstacles) It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated. @@ -136,11 +136,47 @@ Currently we provide three parameters to tune the lateral acceleration constrain You can change these parameters in rosparam in the table below. -| param name | default value | -| ----------------------------------------- | -------------- | -| `check_lateral_acceleration_constraints_` | `false` [bool] | -| `max_lateral_accel` | `2.0` [m/s^2] | -| `min_acceleration_before_curve` | `-2.0` [m/s^2] | +| param name | default value | +| ---------------------------------------- | -------------- | +| `check_lateral_acceleration_constraints` | `false` [bool] | +| `max_lateral_accel` | `2.0` [m/s^2] | +| `min_acceleration_before_curve` | `-2.0` [m/s^2] | + +## Using Vehicle Acceleration for Path Prediction (for Vehicle Obstacles) + +By default, the `map_based_prediction` module uses the current obstacle's velocity to compute its predicted path length. However, it is possible to use the obstacle's current acceleration to calculate its predicted path's length. + +### Decaying Acceleration Model + +Since this module tries to predict the vehicle's path several seconds into the future, it is not practical to consider the current vehicle's acceleration as constant (it is not assumed the vehicle will be accelerating for `prediction_time_horizon` seconds after detection). Instead, a decaying acceleration model is used. With the decaying acceleration model, a vehicle's acceleration is modeled as: + +$\ a(t) = a\_{t0} \cdot e^{-\lambda \cdot t} $ + +where $\ a\_{t0} $ is the vehicle acceleration at the time of detection $\ t0 $, and $\ \lambda $ is the decay constant $\ \lambda = \ln(2) / hl $ and $\ hl $ is the exponential's half life. + +Furthermore, the integration of $\ a(t) $ over time gives us equations for velocity, $\ v(t) $ and distance $\ x(t) $ as: + +$\ v(t) = v*{t0} + a*{t0} \* (1/\lambda) \cdot (1 - e^{-\lambda \cdot t}) $ + +and + +$\ x(t) = x*{t0} + (v*{t0} + a*{t0} \* (1/\lambda)) \cdot t + a*{t0}(1/λ^2)(e^{-\lambda \cdot t} - 1) $ + +With this model, the influence of the vehicle's detected instantaneous acceleration on the predicted path's length is diminished but still considered. This feature also considers that the obstacle might not accelerate past its road's speed limit (multiplied by a tunable factor). + +Currently, we provide three parameters to tune the use of obstacle acceleration for path prediction: + +- `use_vehicle_acceleration`: to enable the feature. +- `acceleration_exponential_half_life`: The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds. +- `speed_limit_multiplier`: Set the vehicle type obstacle's maximum predicted speed as the legal speed limit in that lanelet times this value. This value should be at least equal or greater than 1.0. + +You can change these parameters in `rosparam` in the table below. + +| Param Name | Default Value | +| ------------------------------------ | -------------- | +| `use_vehicle_acceleration` | `false` [bool] | +| `acceleration_exponential_half_life` | `2.5` [s] | +| `speed_limit_multiplier` | `1.5` [] | ### Path prediction for crosswalk users 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 fe4d2a51ec6f3..fdd64174de9be 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -16,6 +16,9 @@ check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths min_acceleration_before_curve: -2.0 # [m/ss] min acceleration a vehicle might take to decelerate before a curve + 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 # 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 02ca62a61717c..f61dc75ffb85b 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -89,6 +90,7 @@ struct LaneletData struct PredictedRefPath { float probability; + double speed_limit; PosePath path; Maneuver maneuver; }; @@ -175,6 +177,10 @@ class MapBasedPredictionNode : public rclcpp::Node double max_lateral_accel_; double min_acceleration_before_curve_; + bool use_vehicle_acceleration_; + double speed_limit_multiplier_; + double acceleration_exponential_half_life_; + // Stop watch StopWatch stop_watch_; @@ -231,7 +237,8 @@ class MapBasedPredictionNode : public rclcpp::Node void addReferencePaths( const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, const float path_probability, const ManeuverProbability & maneuver_probability, - const Maneuver & maneuver, std::vector & reference_paths); + const Maneuver & maneuver, std::vector & reference_paths, + const double speed_limit = 0.0); std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); void updateFuturePossibleLanelets( @@ -316,8 +323,11 @@ class MapBasedPredictionNode : public rclcpp::Node }; inline bool isLateralAccelerationConstraintSatisfied( - const TrajectoryPoints & trajectory [[maybe_unused]], const double delta_time) + const TrajectoryPoints & trajectory, const double delta_time) { + constexpr double epsilon = 1E-6; + if (delta_time < epsilon) throw std::invalid_argument("delta_time must be a positive value"); + if (trajectory.size() < 3) return true; const double max_lateral_accel_abs = std::fabs(max_lateral_accel_); @@ -343,19 +353,17 @@ class MapBasedPredictionNode : public rclcpp::Node delta_theta += 2.0 * M_PI; } - const double yaw_rate = std::max(delta_theta / delta_time, 1.0E-5); + const double yaw_rate = std::max(std::abs(delta_theta / delta_time), 1.0E-5); const double current_speed = std::abs(trajectory.at(i).longitudinal_velocity_mps); // Compute lateral acceleration const double lateral_acceleration = std::abs(current_speed * yaw_rate); if (lateral_acceleration < max_lateral_accel_abs) continue; - const double v_curvature_max = std::sqrt(max_lateral_accel_abs / yaw_rate); const double t = (v_curvature_max - current_speed) / min_acceleration_before_curve_; // acc is negative const double distance_to_slow_down = current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2); - if (distance_to_slow_down > arc_length) return false; } return true; diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 4da4f62be2ede..6dfc4a8db9e20 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -91,7 +91,7 @@ class PathGenerator PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths); + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0); PredictedPath generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; @@ -99,17 +99,30 @@ class PathGenerator PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; + void setUseVehicleAcceleration(const bool use_vehicle_acceleration) + { + use_vehicle_acceleration_ = use_vehicle_acceleration; + } + + void setAccelerationHalfLife(const double acceleration_exponential_half_life) + { + acceleration_exponential_half_life_ = acceleration_exponential_half_life; + } + private: // Parameters double time_horizon_; double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; + bool use_vehicle_acceleration_; + double acceleration_exponential_half_life_; // Member functions PredictedPath generateStraightPath(const TrackedObject & object) const; - PredictedPath generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path); + PredictedPath generatePolynomialPath( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); FrenetPath generateFrenetPath( const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); @@ -125,7 +138,8 @@ class PathGenerator const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path); - FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path); + FrenetPoint getFrenetPoint( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); }; } // namespace map_based_prediction 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 75c1d61e0a19c..c949eae21aeff 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include @@ -385,11 +384,7 @@ bool withinRoadLanelet( const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, const bool use_yaw_information = false) { - using Point = boost::geometry::model::d2::point_xy; - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const Point p_object{obj_pos.x, obj_pos.y}; - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); // nearest lanelet constexpr double search_radius = 10.0; // [m] @@ -613,16 +608,16 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw // constraints const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; + + constexpr float high_speed_threshold = tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); const bool high_speed_object = abs_speed > high_speed_threshold; - - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; // high speed object outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; // temporary disabled if (high_speed_object) return label; // Do nothing for now @@ -788,10 +783,18 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ prediction_time_horizon_rate_for_validate_lane_length_ = declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); + use_vehicle_acceleration_ = declare_parameter("use_vehicle_acceleration"); + speed_limit_multiplier_ = declare_parameter("speed_limit_multiplier"); + acceleration_exponential_half_life_ = + declare_parameter("acceleration_exponential_half_life"); + path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); + path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); + sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); @@ -817,6 +820,13 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); updateParam( parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_); + updateParam(parameters, "use_vehicle_acceleration", use_vehicle_acceleration_); + updateParam(parameters, "speed_limit_multiplier", speed_limit_multiplier_); + updateParam( + parameters, "acceleration_exponential_half_life", acceleration_exponential_half_life_); + + path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); + path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -1010,7 +1020,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path); + yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1555,14 +1565,63 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); + // Using a decaying acceleration model. Consult the README for more information about the model. + const double obj_acc = (use_vehicle_acceleration_) + ? std::hypot( + object.kinematics.acceleration_with_covariance.accel.linear.x, + object.kinematics.acceleration_with_covariance.accel.linear.y) + : 0.0; + const double t_h = prediction_time_horizon_; + const double λ = std::log(2) / acceleration_exponential_half_life_; + + auto get_search_distance_with_decaying_acc = [&]() -> double { + const double acceleration_distance = + obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1); + double search_dist = acceleration_distance + obj_vel * t_h; + return search_dist; + }; + + auto get_search_distance_with_partial_acc = [&](const double final_speed) -> double { + constexpr double epsilon = 1E-5; + if (std::abs(obj_acc) < epsilon) { + // Assume constant speed + return obj_vel * t_h; + } + // Time to reach final speed + const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc); + // It is assumed the vehicle accelerates until final_speed is reached and + // then continues at constant speed for the rest of the time horizon + const double search_dist = + // Distance covered while accelerating + obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + + obj_vel * t_f + + // Distance covered at constant speed + final_speed * (t_h - t_f); + return search_dist; + }; + std::vector all_ref_paths; + for (const auto & current_lanelet_data : current_lanelets_data) { - // parameter for lanelet::routing::PossiblePathsParams - const double search_dist = prediction_time_horizon_ * obj_vel + - lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); + const lanelet::traffic_rules::SpeedLimitInformation limit = + traffic_rules_ptr_->speedLimit(current_lanelet_data.lanelet); + const double legal_speed_limit = static_cast(limit.speedLimit.value()); + + double final_speed_after_acceleration = + obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h)); + + const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_; + const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit; + const bool object_has_surpassed_limit_already = obj_vel > final_speed_limit; + + double search_dist = (final_speed_surpasses_limit && !object_has_surpassed_limit_already) + ? get_search_distance_with_partial_acc(final_speed_limit) + : get_search_distance_with_decaying_acc(); + search_dist += lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); + lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true}; const double validate_time_horizon = - prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_; + t_h * prediction_time_horizon_rate_for_validate_lane_length_; // lambda function to get possible paths for isolated lanelet // isolated is often caused by lanelet with no connection e.g. shoulder-lane @@ -1644,7 +1703,8 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( // Step4. add candidate reference paths to the all_ref_paths const float path_prob = current_lanelet_data.probability; const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) { - addReferencePaths(object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths); + addReferencePaths( + object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths, final_speed_limit); }; addReferencePathsLocal(left_paths, Maneuver::LEFT_LANE_CHANGE); addReferencePathsLocal(right_paths, Maneuver::RIGHT_LANE_CHANGE); @@ -1966,7 +2026,8 @@ void MapBasedPredictionNode::updateFuturePossibleLanelets( void MapBasedPredictionNode::addReferencePaths( const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, const float path_probability, const ManeuverProbability & maneuver_probability, - const Maneuver & maneuver, std::vector & reference_paths) + const Maneuver & maneuver, std::vector & reference_paths, + const double speed_limit) { if (!candidate_paths.empty()) { updateFuturePossibleLanelets(object, candidate_paths); @@ -1976,6 +2037,7 @@ void MapBasedPredictionNode::addReferencePaths( predicted_path.probability = maneuver_probability.at(maneuver) * path_probability; predicted_path.path = converted_path; predicted_path.maneuver = maneuver; + predicted_path.speed_limit = speed_limit; reference_paths.push_back(predicted_path); } } diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 5cb7e186b7cc4..413a0e233186b 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -149,20 +149,20 @@ PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths) + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) { if (ref_paths.size() < 2) { return generateStraightPath(object); } - return generatePolynomialPath(object, ref_paths); + return generatePolynomialPath(object, ref_paths, speed_limit); } PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const double ep = 0.001; + constexpr double ep = 0.001; const double duration = time_horizon_ + ep; PredictedPath path; @@ -178,11 +178,11 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path); + const auto current_point = getFrenetPoint(object, ref_path, speed_limit); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -384,7 +384,8 @@ PredictedPath PathGenerator::convertToPredictedPath( return predicted_path; } -FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const PosePath & ref_path) +FrenetPoint PathGenerator::getFrenetPoint( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -400,10 +401,82 @@ FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const Po static_cast(tf2::getYaw(ref_path.at(nearest_segment_idx).orientation)); const float delta_yaw = obj_yaw - lane_yaw; + const float ax = + (use_vehicle_acceleration_) + ? static_cast(object.kinematics.acceleration_with_covariance.accel.linear.x) + : 0.0; + const float ay = + (use_vehicle_acceleration_) + ? static_cast(object.kinematics.acceleration_with_covariance.accel.linear.y) + : 0.0; + + // Using a decaying acceleration model. Consult the README for more information about the model. + const double t_h = time_horizon_; + const float λ = std::log(2) / acceleration_exponential_half_life_; + + auto have_same_sign = [](double a, double b) -> bool { + return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0); + }; + + auto get_acceleration_adjusted_velocity = [&](const double v, const double a) { + constexpr double epsilon = 1E-5; + if (std::abs(a) < epsilon) { + // Assume constant speed + return v; + } + // Get velocity after time horizon + const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h)); + + // If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at + // most stop, not reverse its direction) + if (!have_same_sign(terminal_velocity, v)) { + // we assume a forwards moving vehicle will not decelerate to 0 and then move backwards + // if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed -> + // time t_stop + + // 0 = Vo + acc(1/λ)(1-e^(-λt_stop)) + // e^(-λt_stop) = 1 - (-Vo* λ)/acc + // t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc) + // t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc) + auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a)); + + // Calculate the distance traveled until stopping + auto distance_to_reach_zero_speed = + v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1); + // Output an equivalent constant speed + return distance_to_reach_zero_speed / t_h; + } + + // if the vehicle speed limit is not surpassed we return an equivalent speed = x(T) / T + // alternatively, if the vehicle is still accelerating and has surpassed the speed limit. + // assume it will continue accelerating (reckless driving) + const bool object_has_surpassed_limit_already = v > speed_limit; + if (terminal_velocity < speed_limit || object_has_surpassed_limit_already) + return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1); + + // It is assumed the vehicle accelerates until final_speed is reached and + // then continues at constant speed for the rest of the time horizon + // So, we calculate the time it takes to reach the speed limit and compute how far the vehicle + // would go if it accelerated until reaching the speed limit, and then continued at a constant + // speed. + const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a); + const double distance_covered = + // Distance covered while accelerating + a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f + + // Distance covered at constant speed for the rest of the horizon time + speed_limit * (t_h - t_f); + return distance_covered / t_h; + }; + + const float acceleration_adjusted_velocity_x = get_acceleration_adjusted_velocity(vx, ax); + const float acceleration_adjusted_velocity_y = get_acceleration_adjusted_velocity(vy, ay); + frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point); - frenet_point.s_vel = vx * std::cos(delta_yaw) - vy * std::sin(delta_yaw); - frenet_point.d_vel = vx * std::sin(delta_yaw) + vy * std::cos(delta_yaw); + frenet_point.s_vel = acceleration_adjusted_velocity_x * std::cos(delta_yaw) - + acceleration_adjusted_velocity_y * std::sin(delta_yaw); + frenet_point.d_vel = acceleration_adjusted_velocity_x * std::sin(delta_yaw) + + acceleration_adjusted_velocity_y * std::cos(delta_yaw); frenet_point.s_acc = 0.0; frenet_point.d_acc = 0.0; diff --git a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp similarity index 95% rename from perception/probabilistic_occupancy_grid_map/include/cost_value.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp index e0f18cedcdff1..880297a1210ed 100644 --- a/perception/probabilistic_occupancy_grid_map/include/cost_value.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/cost_value.hpp @@ -48,8 +48,8 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef COST_VALUE_HPP_ -#define COST_VALUE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ #include @@ -103,4 +103,4 @@ static const CostTranslationTable cost_translation_table; static const InverseCostTranslationTable inverse_cost_translation_table; } // namespace occupancy_cost_value -#endif // COST_VALUE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__COST_VALUE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp similarity index 88% rename from perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp index 13829b4910d96..1f0553878ef5a 100644 --- a/perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#define FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -63,4 +63,4 @@ unsigned char singleFrameOccupancyFusion( const std::vector & reliability); } // namespace fusion_policy -#endif // FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp similarity index 87% rename from perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp index 6bea5b2a16f72..8f09764688055 100644 --- a/perception/probabilistic_occupancy_grid_map/include/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#define FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ -#include "fusion/single_frame_fusion_policy.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -124,4 +124,4 @@ class GridMapFusionNode : public rclcpp::Node } // namespace synchronized_grid_map_fusion -#endif // FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp similarity index 84% rename from perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index 3a0f1f5ea7433..a8adea6e700e5 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -102,4 +102,4 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node } // namespace occupancy_grid_map -#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp similarity index 92% rename from perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp index a9bbc3fda6ab4..d2210cf9c348a 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ -#define LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ #include #include @@ -91,4 +91,4 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__LASERSCAN_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp similarity index 92% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp index 8f8e1e503d388..f01b2d74e160b 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp @@ -49,8 +49,8 @@ * David V. Lu!! *********************************************************************/ -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ #include #include @@ -92,4 +92,4 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_BASE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp similarity index 75% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp index 5755cd2c889be..298f327d632d7 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" namespace costmap_2d { @@ -44,4 +44,4 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_FIXED_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp similarity index 78% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp index bc278772e9737..67b51d6184c8c 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" #include @@ -52,4 +52,4 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface } // namespace costmap_2d -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_PROJECTIVE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp similarity index 83% rename from perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index e34899bb98b0c..93486e0ca56af 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#define POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -95,4 +95,4 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node } // namespace occupancy_grid_map -#endif // POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP__POINTCLOUD_BASED_OCCUPANCY_GRID_MAP_NODE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp similarity index 75% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp index d8f3cb6556bf2..3a921035ef219 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ -#include "updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" #include #include @@ -39,4 +39,4 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_BINARY_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp similarity index 71% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp index 306f8988acab7..f9f100f285d38 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ -#include "fusion/single_frame_fusion_policy.hpp" -#include "updater/occupancy_grid_map_updater_interface.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include @@ -45,4 +45,4 @@ class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_LOG_ODDS_BAYES_FILTER_UPDATER_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp similarity index 78% rename from perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp index 9305313ddc2fc..e85edf2245ef3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/updater/occupancy_grid_map_updater_interface.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#define UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -38,4 +38,4 @@ class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D } // namespace costmap_2d -#endif // UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OCCUPANCY_GRID_MAP_UPDATER_INTERFACE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp similarity index 93% rename from perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp rename to perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp index 46bd14b843dae..0950272dac470 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -119,4 +119,4 @@ bool extractCommonPointCloud( unsigned char getApproximateOccupancyState(const unsigned char & value); } // namespace utils -#endif // UTILS__UTILS_HPP_ +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index b99335b0e09ef..62cfa4bcb5228 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -115,7 +115,7 @@ def launch_setup(context, *args, **kwargs): ] occupancy_grid_map_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -126,7 +126,7 @@ def launch_setup(context, *args, **kwargs): load_composable_nodes = LoadComposableNodes( composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) @@ -174,7 +174,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_obstacle_pointcloud", "false"), add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), add_launch_arg("use_pointcloud_container", "false"), - add_launch_arg("container_name", "occupancy_grid_map_container"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), + add_launch_arg("individual_container_name", "occupancy_grid_map_container"), set_container_executable, set_container_mt_executable, ] diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 49bf228905dcc..29435c4ea8e24 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -64,7 +64,7 @@ def launch_setup(context, *args, **kwargs): ] occupancy_grid_map_container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -75,7 +75,7 @@ def launch_setup(context, *args, **kwargs): load_composable_nodes = LoadComposableNodes( composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) @@ -103,7 +103,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "false"), add_launch_arg("use_intra_process", "true"), add_launch_arg("use_pointcloud_container", "false"), - add_launch_arg("container_name", "occupancy_grid_map_container"), + add_launch_arg("pointcloud_container_name", "pointcloud_container"), + add_launch_arg("individual_container_name", "occupancy_grid_map_container"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), add_launch_arg("output", "occupancy_grid"), diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp index 75e8791a04bca..6ac681eff4916 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" namespace fusion_policy { diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 6738286ae5592..a88e65e712ac1 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fusion/synchronized_grid_map_fusion_node.hpp" +#include "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" namespace synchronized_grid_map_fusion { diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index b4505eedddd21..777c180fe1a3a 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp index 59f562cb58f35..3d02be9ca7156 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -49,9 +49,9 @@ * David V. Lu!! *********************************************************************/ -#include "laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" +#include "probabilistic_occupancy_grid_map/laserscan_based_occupancy_grid_map/occupancy_grid_map.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 8652cfa34d96c..3d176e41583a0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -49,10 +49,10 @@ * David V. Lu!! *********************************************************************/ -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index 56aeea30e0773..00b3e42fdf392 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 20a5770e37fdb..64b76a2488e24 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "cost_value.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index f6369602b8890..82da92da7f146 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp" -#include "cost_value.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" -#include "pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp index 16de17b1c2e61..40f538a64f6e9 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_binary_bayes_filter_updater.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp index e45f3d0686fbc..f3e306f262bf0 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index b876004d89b0a..8009a503167ea 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" #include diff --git a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md index e1046fa24719d..6f4d4e22aa26e 100644 --- a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md +++ b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -105,7 +105,7 @@ You need to generate OGMs in each sensor frame before achieving grid map fusion. - + @@ -146,7 +146,7 @@ You can include this launch file like the following. - + diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp index baf94fabfd2a1..c41c809a92b0c 100644 --- a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "cost_value.hpp" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" -#include "gtest/gtest.h" +#include // Test the CostTranslationTable and InverseCostTranslationTable functions using occupancy_cost_value::cost_translation_table; diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp index 2501a361fba58..6b3dc8a2ebcef 100644 --- a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "cost_value.hpp" -#include "fusion/single_frame_fusion_policy.hpp" -#include "gtest/gtest.h" +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" + +#include // Test the log-odds update rule TEST(FusionPolicyTest, TestLogOddsUpdateRule) diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 367dcee807e7b..4bc3dca0a67bd 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +// autoware +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" + #include #include @@ -21,9 +24,6 @@ #include #include -// autoware -#include "utils/utils.hpp" - // create pointcloud function pcl::PointCloud createPCLPointCloudWithIteratedHeight(const size_t width) { diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index fb8f117c82245..fa08eb08f521a 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -23,7 +23,7 @@ This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-p - `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 "false" + - 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]. 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 a6a3f394b1f40..cc2bb80c6f4f7 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 @@ -4,7 +4,7 @@ - + diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 5ae6002cd7c3b..8eb7a15b5a885 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -64,4 +64,5 @@ rclcpp_components_register_node(shape_estimation_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/shape_estimation/README.md b/perception/shape_estimation/README.md index c50d66b546213..b635631381cc3 100644 --- a/perception/shape_estimation/README.md +++ b/perception/shape_estimation/README.md @@ -36,11 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ---- | ------------- | --------------------------------------------------- | -| `use_corrector` | bool | true | The flag to apply rule-based filter | -| `use_filter` | bool | true | The flag to apply rule-based corrector | -| `use_vehicle_reference_yaw` | bool | true | The flag to use vehicle reference yaw for corrector | +{{ json_to_markdown("perception/shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml new file mode 100644 index 0000000000000..253516fffe0d4 --- /dev/null +++ b/perception/shape_estimation/config/shape_estimation.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + use_corrector: true + use_filter: true + use_vehicle_reference_yaw: false + use_vehicle_reference_shape_size: false + use_boost_bbox_optimizer: false diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 8e90e3ea57cc0..65d1944417cc0 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -1,18 +1,16 @@ - - + + + - - - - + diff --git a/perception/shape_estimation/schema/shape_estimation.schema.json b/perception/shape_estimation/schema/shape_estimation.schema.json new file mode 100644 index 0000000000000..d81bfa636a923 --- /dev/null +++ b/perception/shape_estimation/schema/shape_estimation.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Shape Estimation Node", + "type": "object", + "definitions": { + "shape_estimation": { + "type": "object", + "properties": { + "use_corrector": { + "type": "boolean", + "description": "The flag to apply rule-based corrector.", + "default": "true" + }, + "use_filter": { + "type": "boolean", + "description": "The flag to apply rule-based filter", + "default": "true" + }, + "use_vehicle_reference_yaw": { + "type": "boolean", + "description": "The flag to use vehicle reference yaw for corrector", + "default": "false" + }, + "use_vehicle_reference_shape_size": { + "type": "boolean", + "description": "The flag to use vehicle reference shape size", + "default": "false" + }, + "use_boost_bbox_optimizer": { + "type": "boolean", + "description": "The flag to use boost bbox optimizer", + "default": "false" + } + }, + "required": [ + "use_corrector", + "use_filter", + "use_vehicle_reference_yaw", + "use_vehicle_reference_shape_size", + "use_boost_bbox_optimizer" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/shape_estimation" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 987cf8106c99e..624ca604d8fbf 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -42,11 +42,11 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option "input", rclcpp::QoS{1}, std::bind(&ShapeEstimationNode::callback, this, _1)); pub_ = create_publisher("objects", rclcpp::QoS{1}); - bool use_corrector = declare_parameter("use_corrector", true); - bool use_filter = declare_parameter("use_filter", true); - use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw", true); - use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size", true); - bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer", false); + bool use_corrector = declare_parameter("use_corrector"); + bool use_filter = declare_parameter("use_filter"); + use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); + use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); + bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); diff --git a/perception/tensorrt_yolo/config/yolov3.param.yaml b/perception/tensorrt_yolo/config/yolov3.param.yaml index 5ce50f3acfa96..d97505dae5020 100644 --- a/perception/tensorrt_yolo/config/yolov3.param.yaml +++ b/perception/tensorrt_yolo/config/yolov3.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [116.0, 90.0, 156.0, 198.0, 373.0, 326.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 10.0, 13.0, 16.0, 30.0, 33.0, 23.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml b/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml index 2ee53ee68f764..8071da9dcac06 100644 --- a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml +++ b/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [81.0, 82.0, 135.0, 169.0, 344.0, 319.0, 23.0, 27.0, 37.0, 58.0, 81.0, 82.0] scale_x_y: [1.05, 1.05] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov4.param.yaml b/perception/tensorrt_yolo/config/yolov4.param.yaml index 6122af667c470..9edc0fc6ce708 100644 --- a/perception/tensorrt_yolo/config/yolov4.param.yaml +++ b/perception/tensorrt_yolo/config/yolov4.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [12.0, 16.0, 19.0, 36.0, 40.0, 28.0, 36.0, 75.0, 76.0, 55.0, 72.0, 146.0, 142.0, 110.0, 192.0, 243.0, 459.0, 401.0] scale_x_y: [1.2, 1.1, 1.05] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov5l.param.yaml b/perception/tensorrt_yolo/config/yolov5l.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5l.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5l.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5m.param.yaml b/perception/tensorrt_yolo/config/yolov5m.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5m.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5m.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5s.param.yaml b/perception/tensorrt_yolo/config/yolov5s.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5s.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5s.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5x.param.yaml b/perception/tensorrt_yolo/config/yolov5x.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5x.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5x.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml rename to planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp index d996555365166..75fa67e7fe1a3 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp @@ -35,7 +35,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager public: AvoidanceByLaneChangeModuleManager() : LaneChangeModuleManager( - "avoidance_by_lc", route_handler::Direction::NONE, + "avoidance_by_lane_change", route_handler::Direction::NONE, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { } diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 3f810710ef37b..4bdcb51f1eab5 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -37,7 +37,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) initInterface(node, {"left", "right"}); // init lane change manager - LaneChangeModuleManager::init(node); + LaneChangeModuleManager::initParams(node); const auto avoidance_params = getParameter(node); AvoidanceByLCParameters p(avoidance_params); 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 b76a9e5645b48..6107314bc2824 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 @@ -187,7 +187,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( } void AvoidanceByLaneChange::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, DebugData & debug) const + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -227,7 +227,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( [&](const auto & object) { return createObjectData(data, object); }); } - utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p); + utils::avoidance::filterTargetObjects( + target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance, + planner_data_, p); } ObjectData AvoidanceByLaneChange::createObjectData( diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 581aafa6c860c..1167fa4414752 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -70,7 +70,7 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + "/config/avoidance.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + - "/config/avoidance_by_lc.param.yaml"}); + "/config/avoidance_by_lane_change.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/behavior_path_avoidance_module/README.md index 20c0985f0f333..122ad7adcce9e 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -833,7 +833,8 @@ namespace: `avoidance.target_filtering.` | object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | | object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | | object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | +| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not return center line. | 20.0 | +| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do not return center line. | 20.0 | | object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | | object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | | object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 2d38cebd591f5..b300de2236fcf 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -122,7 +122,8 @@ motorcycle: true # [-] pedestrian: true # [-] # detection range - object_check_goal_distance: 20.0 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] object_check_shiftable_ratio: 0.6 # [-] 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 1d1494b7719c3..9b5ae3cb4db9e 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 @@ -164,10 +164,14 @@ struct AvoidanceParameters double object_check_backward_distance{0.0}; double object_check_yaw_deviation{0.0}; - // if the distance between object and goal position is less than this parameter, the module ignore - // the object. + // if the distance between object and goal position is less than this parameter, the module do not + // return center line. double object_check_goal_distance{0.0}; + // if the distance between object and return position is less than this parameter, the module do + // not return center line. + double object_check_return_pose_distance{0.0}; + // use in judge whether the vehicle is parking object on road shoulder double object_check_shiftable_ratio{0.0}; @@ -462,14 +466,14 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(AvoidLine avoid_line, AvoidLine return_line) + AvoidOutline(AvoidLine avoid_line, const std::optional return_line) : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } AvoidLine avoid_line{}; - AvoidLine return_line{}; + std::optional return_line{}; AvoidLineArray middle_lines{}; }; @@ -588,7 +592,7 @@ struct ShiftLineData */ struct DebugData { - geometry_msgs::msg::Polygon detection_area; + std::vector detection_areas; lanelet::ConstLineStrings3d bounds; 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 b9af50ce76eb5..9f2fdf7ab96d9 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 @@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_check_return_pose_distance = + getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); p.threshold_distance_object_is_on_center = getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = 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 889d48c481b6f..0bb480bfa26b1 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 @@ -112,13 +112,12 @@ class AvoidanceModule : public SceneModuleInterface */ void updateRegisteredRTCStatus(const PathWithLaneId & path) { - const Point ego_position = planner_data_->self_odometry->pose.pose.position; + const auto ego_idx = planner_data_->findEgoIndex(path.points); for (const auto & left_shift : left_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); + const double finish_distance = start_distance + left_shift.relative_longitudinal; rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -130,9 +129,8 @@ class AvoidanceModule : public SceneModuleInterface for (const auto & right_shift : right_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); + const double finish_distance = start_distance + right_shift.relative_longitudinal; rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -399,6 +397,7 @@ class AvoidanceModule : public SceneModuleInterface UUID uuid; Pose start_pose; Pose finish_pose; + double relative_longitudinal{0.0}; }; using RegisteredShiftLineArray = std::vector; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index dc602edcc8b86..cbf68ada44abb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -77,11 +77,6 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); - -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); - lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); @@ -128,12 +123,7 @@ void compensateDetectionLost( ObjectDataArray & other_objects); void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); - -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); @@ -157,9 +147,10 @@ std::vector getSafetyCheckTargetObjects( DebugData & debug); std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug); + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index ee97efe37d440..f088b9228d964 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -608,10 +608,15 @@ MarkerArray createDebugMarkerArray( addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } + // detection area + size_t i = 0; + for (const auto & detection_area : debug.detection_areas) { + add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + } + // misc { add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); add(laneletsAsTriangleMarkerArray( "drivable_lanes", transformToLanelets(data.drivable_lanes), diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 61941eeee6f00..bc1d8c6d75424 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -287,21 +287,18 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + using utils::avoidance::fillAvoidanceNecessity; using utils::avoidance::fillObjectStoppableJudge; using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; - - // Add margin in order to prevent avoidance request chattering only when the module is running. - const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || - getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + using utils::avoidance::separateObjectsByPath; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. - const auto sparse_resample_path = utils::resamplePathWithSpline( - helper_->getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::avoidance::separateObjectsByPath( - sparse_resample_path, planner_data_, data, parameters_, helper_->getForwardDetectionRange(), - is_running, debug); + constexpr double MARGIN = 10.0; + const auto forward_detection_range = helper_->getForwardDetectionRange(); + const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath( + helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_, + data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -316,11 +313,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } // Filter out the objects to determine the ones to be avoided. - filterTargetObjects(objects, data, debug, planner_data_, parameters_); + filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); o.to_stop_line = calcDistanceToStopLine(o); fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); }); @@ -380,21 +379,10 @@ ObjectData AvoidanceModule::createObjectData( utils::avoidance::fillInitialPose(object_data, detected_objects_); // Calc lateral deviation from path to target object. - object_data.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, data.reference_path, object_data.overhang_pose.position); - - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity( - object_data, registered_objects_, vehicle_width, parameters_); - return object_data; } @@ -703,18 +691,6 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const bool limit_to_max_velocity = false; - const auto ego_predicted_path_params = - std::make_shared( - parameters_->ego_predicted_path_params); - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); - const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - true, limit_to_max_velocity); - const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - false, limit_to_max_velocity); - const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { @@ -741,6 +717,22 @@ bool AvoidanceModule::isSafePath( const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoid_data_, planner_data_, parameters_, is_right_shift.value(), debug); + if (safety_check_target_objects.empty()) { + return true; + } + + const bool limit_to_max_velocity = false; + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); + for (const auto & object : safety_check_target_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); @@ -793,15 +785,16 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); - } - for (const auto & sp : generator_.getRawRegisteredShiftLine()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); + auto lines = path_shifter_.getShiftLines(); + if (lines.empty()) { + return max_dist; } - return max_dist; + std::sort(lines.begin(), lines.end(), [](const auto & a, const auto & b) { + return a.start_idx < b.start_idx; + }); + return std::max( + max_dist, + calcSignedArcLength(previous_path.points, lines.front().start.position, getEgoPosition())); }(); const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. @@ -1029,11 +1022,14 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) const auto sl = helper_->getMainShiftLine(shift_lines); const auto sl_front = shift_lines.front(); const auto sl_back = shift_lines.back(); + const auto relative_longitudinal = sl_back.end_longitudinal - sl_front.start_longitudinal; if (helper_->getRelativeShiftToPath(sl) > 0.0) { - left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); + left_shift_array_.push_back( + {uuid_map_.at("left"), sl_front.start, sl_back.end, relative_longitudinal}); } else if (helper_->getRelativeShiftToPath(sl) < 0.0) { - right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); + right_shift_array_.push_back( + {uuid_map_.at("right"), sl_front.start, sl_back.end, relative_longitudinal}); } uuid_map_.at("left") = generateUUID(); @@ -1150,15 +1146,19 @@ bool AvoidanceModule::isValidShiftLine( const size_t start_idx = shift_lines.front().start_idx; const size_t end_idx = shift_lines.back().end_idx; + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(avoid_data_.left_bound); + const auto right_bound = lanelet::utils::to2D(avoid_data_.right_bound); for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); + const auto p = getPoint(path.points.at(i)); lanelet::BasicPoint2d basic_point{p.x, p.y}; const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; const auto THRESHOLD = minimum_distance + std::abs(shift_length); - if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { RCLCPP_DEBUG_THROTTLE( getLogger(), *clock_, 1000, "following latest new shift line may cause deviation from drivable area."); 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 dabb0d7257555..6196b3e209d11 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -81,7 +81,9 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) AvoidLineArray ret{}; for (const auto & outline : outlines) { ret.push_back(outline.avoid_line); - ret.push_back(outline.return_line); + if (outline.return_line.has_value()) { + ret.push_back(outline.return_line.value()); + } std::for_each( outline.middle_lines.begin(), outline.middle_lines.end(), @@ -341,7 +343,30 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_return.object_on_right = utils::avoidance::isOnRight(o); } - if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + const bool skip_return_shift = [&]() { + if (!utils::isAllowedGoalModification(data_->route_handler)) { + return false; + } + const auto goal_pose = data_->route_handler->getGoalPose(); + const double goal_longitudinal_distance = + motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position); + const bool is_return_shift_to_goal = + std::abs(al_return.end_longitudinal - goal_longitudinal_distance) < + parameters_->object_check_return_pose_distance; + if (is_return_shift_to_goal) { + return true; + } + const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; + const bool has_object_near_goal = + tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) < + parameters_->object_check_goal_distance; + return has_object_near_goal; + }(); + + if (skip_return_shift) { + // if the object is close to goal or ego is about to return near GOAL, do not return + outlines.emplace_back(al_avoid, std::nullopt); + } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { outlines.emplace_back(al_avoid, al_return); } else { o.reason = "InvalidShiftLine"; @@ -637,35 +662,43 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( auto & next_outline = outlines.at(i); const auto & return_line = last_outline.return_line; - const auto & avoid_line = next_outline.avoid_line; + if (!return_line.has_value()) { + ret.push_back(outlines.at(i)); + break; + } - if (no_conflict(return_line, avoid_line)) { + const auto & avoid_line = next_outline.avoid_line; + if (no_conflict(return_line.value(), avoid_line)) { ret.push_back(outlines.at(i)); continue; } - const auto merged_shift_line = merge(return_line, avoid_line, generateUUID()); + const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID()); if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { ret.push_back(outlines.at(i)); continue; } - if (same_side_shift(return_line, avoid_line)) { + if (same_side_shift(return_line.value(), avoid_line)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + if ( + within(return_line.value(), avoid_line.end_idx) && + within(avoid_line, return_line->start_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + if ( + within(return_line.value(), avoid_line.start_idx) && + within(avoid_line, return_line->end_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); @@ -686,7 +719,10 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( for (auto & outline : ret) { if (outline.middle_lines.empty()) { - const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID()); + const auto new_line = + outline.return_line.has_value() + ? fill(outline.avoid_line, outline.return_line.value(), generateUUID()) + : outline.avoid_line; outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -701,8 +737,11 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); - if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { - const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID()); + if ( + outline.return_line.has_value() && + outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) { + const auto new_line = + fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -973,6 +1012,20 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const bool has_candidate_point = !ret.empty(); const bool has_registered_point = last_.has_value(); + if (utils::isAllowedGoalModification(data_->route_handler)) { + const auto has_object_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + data_->route_handler->getGoalPose().position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_object_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "object near goal exists so skip adding return shift"); + return ret; + } + } + const auto exist_unavoidable_object = std::any_of( data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); @@ -1027,6 +1080,21 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( } } + // if last shift line is near the objects, do not add return shift. + if (utils::isAllowedGoalModification(data_->route_handler)) { + const bool has_last_shift_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + last_sl.end.position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_last_shift_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "last shift line is near the objects"); + return ret; + } + } + // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. // Here the return-shift from ego is added for the in case. @@ -1070,8 +1138,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( return ret; } - const auto remaining_distance = std::min( - arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); + const auto remaining_distance = + utils::isAllowedGoalModification(data_->route_handler) + ? data.to_return_point + : std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx); @@ -1101,8 +1172,8 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = - std::clamp(nominal_prepare_distance, helper_->getMinimumPrepareDistance(), last_sl_distance); + double prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / @@ -1122,7 +1193,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; - al.end_longitudinal = arclength_from_ego.at(al.end_idx); + al.end_longitudinal = prepare_distance_scaled; al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; ret.push_back(al); @@ -1136,7 +1207,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.start = data.reference_path.points.at(al.start_idx).point.pose; - al.start_longitudinal = arclength_from_ego.at(al.start_idx); + al.start_longitudinal = prepare_distance_scaled; al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 83e96d83b995f..3a9b5db283304 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -16,7 +16,6 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" @@ -176,7 +175,8 @@ geometry_msgs::msg::Polygon createVehiclePolygon( } Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, + const geometry_msgs::msg::Pose & p3, const geometry_msgs::msg::Pose & p4, const geometry_msgs::msg::Polygon & base_polygon) { Polygon2d one_step_polygon{}; @@ -184,7 +184,7 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); + geometry_tf.transform = pose2transform(p1); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -195,7 +195,29 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); + geometry_tf.transform = pose2transform(p2); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p3); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p4); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -630,7 +652,7 @@ bool isForceAvoidanceTarget( } bool isSatisfiedWithCommonCondition( - ObjectData & object, const AvoidancePlanningData & data, + ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -657,7 +679,7 @@ bool isSatisfiedWithCommonCondition( return false; } - if (object.longitudinal > parameters->object_check_max_forward_distance) { + if (object.longitudinal > forward_detection_range) { object.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; return false; } @@ -678,11 +700,13 @@ bool isSatisfiedWithCommonCondition( return false; } - if ( - object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > - to_goal_distance) { - object.reason = "TooNearToGoal"; - return false; + if (!utils::isAllowedGoalModification(planner_data->route_handler)) { + if ( + object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > + to_goal_distance) { + object.reason = "TooNearToGoal"; + return false; + } } return true; @@ -742,6 +766,9 @@ bool isSatisfiedWithVehicleCondition( } // 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; @@ -818,6 +845,64 @@ std::optional getAvoidMargin( // Step3. nominal case. avoid margin is limited by soft constraint. return std::min(soft_lateral_distance_limit, max_avoid_margin); } + +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data) +{ + using lanelet::utils::to2D; + using tier4_autoware_utils::Point2d; + + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + findNearestIndex(data.reference_path.points, object_pose.position); + const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; + + const auto rh = planner_data->route_handler; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { + return 0.0; + } + + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto & p1_object = object.overhang_pose.position; + const auto p_tmp = + geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); + const auto p2_object = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + + std::vector intersects; + for (size_t i = 1; i < bound.size(); i++) { + const auto p1_bound = + geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); + const auto p2_bound = + geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); + + const auto opt_intersect = + tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); + + if (!opt_intersect) { + continue; + } + + intersects.push_back(opt_intersect.value()); + } + + if (intersects.empty()) { + return 0.0; + } + + std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { + return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); + }); + + object.nearest_bound_point = intersects.front(); + + return calcDistance2d(p1_object, object.nearest_bound_point.value()); +} } // namespace filtering_utils bool isOnRight(const ObjectData & obj) @@ -1114,11 +1199,6 @@ std::vector generateObstaclePolygonsForDrivableArea( return obstacles_for_drivable_area; } -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) -{ - return v * std::cos(calcYawDeviation(p_ref, p_target)); -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -1518,67 +1598,8 @@ void compensateDetectionLost( } } -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - [[maybe_unused]] const std::shared_ptr & parameters) -{ - using lanelet::utils::to2D; - using tier4_autoware_utils::Point2d; - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = - findNearestIndex(data.reference_path.points, object_pose.position); - const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; - - const auto rh = planner_data->route_handler; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { - return 0.0; - } - - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto & p1_object = object.overhang_pose.position; - const auto p_tmp = - geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); - const auto p2_object = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; - - // TODO(Satoshi OTA): check if the basic point is on right or left of bound. - const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; - - std::vector intersects; - for (size_t i = 1; i < bound.size(); i++) { - const auto p1_bound = - geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); - const auto p2_bound = - geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); - - const auto opt_intersect = - tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); - - if (!opt_intersect) { - continue; - } - - intersects.push_back(opt_intersect.value()); - } - - if (intersects.empty()) { - return 0.0; - } - - std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { - return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); - }); - - object.nearest_bound_point = intersects.front(); - - return calcDistance2d(p1_object, object.nearest_bound_point.value()); -} - void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -1593,12 +1614,16 @@ void filterTargetObjects( }; for (auto & o : objects) { - if (!filtering_utils::isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { + if (!filtering_utils::isSatisfiedWithCommonCondition( + o, data, forward_detection_range, planner_data, parameters)) { data.other_objects.push_back(o); continue; } - o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters); + // Find the footprint point closest to the path, set to object_data.overhang_distance. + o.overhang_dist = + calcEnvelopeOverhangDistance(o, data.reference_path, o.overhang_pose.position); + o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { @@ -1663,7 +1688,9 @@ void fillAdditionalInfoFromLongitudinal( { for (auto & outline : outlines) { fillAdditionalInfoFromLongitudinal(data, outline.avoid_line); - fillAdditionalInfoFromLongitudinal(data, outline.return_line); + if (outline.return_line.has_value()) { + fillAdditionalInfoFromLongitudinal(data, outline.return_line.value()); + } std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) { fillAdditionalInfoFromLongitudinal(data, line); @@ -1878,13 +1905,18 @@ std::vector getSafetyCheckTargetObjects( } std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug) + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; + if (reference_path.points.empty() || spline_path.points.empty()) { + return std::make_pair(target_objects, other_objects); + } + double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; @@ -1895,53 +1927,57 @@ std::pair separateObjectsByPath( const auto detection_area = createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); - const auto ego_idx = planner_data->findEgoIndex(path.points); - - Polygon2d attention_area; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p_ego_front = path.points.at(i).point.pose; - const auto & p_ego_back = path.points.at(i + 1).point.pose; - - const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); + const auto ego_idx = planner_data->findEgoIndex(reference_path.points); + const auto arc_length_array = + utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 0.0); + + const auto points_size = std::min(reference_path.points.size(), spline_path.points.size()); + + std::vector detection_areas; + Pose p_reference_ego_front = reference_path.points.front().point.pose; + Pose p_spline_ego_front = spline_path.points.front().point.pose; + double next_longitudinal_distance = parameters->resample_interval_for_output; + for (size_t i = 0; i < points_size; ++i) { + const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; } - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area); - - std::vector unions; - boost::geometry::union_(attention_area, ego_one_step_polygon, unions); - if (!unions.empty()) { - attention_area = unions.front(); - boost::geometry::correct(attention_area); + if (arc_length_array.at(i) < next_longitudinal_distance) { + continue; } + + const auto & p_reference_ego_back = reference_path.points.at(i).point.pose; + const auto & p_spline_ego_back = spline_path.points.at(i).point.pose; + + detection_areas.push_back(createOneStepPolygon( + p_reference_ego_front, p_reference_ego_back, p_spline_ego_front, p_spline_ego_back, + detection_area)); + + p_reference_ego_front = p_reference_ego_back; + p_spline_ego_front = p_spline_ego_back; + + next_longitudinal_distance += parameters->resample_interval_for_output; } - // expand detection area width only when the module is running. - if (is_running) { - constexpr int PER_CIRCLE = 36; - constexpr double MARGIN = 1.0; // [m] - boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); - boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - if (!result.empty()) { - attention_area = result.front(); + std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) { + debug.detection_areas.push_back(toMsg(detection_area, data.reference_pose.position.z)); + }); + + const auto within_detection_area = [&](const auto & obj_polygon) { + for (const auto & detection_area : detection_areas) { + if (!boost::geometry::disjoint(obj_polygon, detection_area)) { + return true; + } } - } - debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); + return false; + }; const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - if (boost::geometry::disjoint(obj_polygon, attention_area)) { + if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { target_objects.objects.push_back(object); @@ -2146,7 +2182,9 @@ double calcDistanceToReturnDeadLine( } // dead line for goal - if (parameters->enable_dead_line_for_goal) { + if ( + !utils::isAllowedGoalModification(planner_data->route_handler) && + parameters->enable_dead_line_for_goal) { if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto to_goal_distance = 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 d2f9cd95066d9..a5380b628387d 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 @@ -382,8 +382,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, - const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, - const double obj_normal_vel, const std::optional & prev_object) const; + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index ca58bbf0adcaa..b43102ecbd16d 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -676,8 +676,8 @@ void DynamicAvoidanceModule::updateTargetObjects() ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape, time_while_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel, - prev_object); + ref_path_points_for_obj_poly, obj_points, object.pose.position, object.vel, is_collision_left, + object.lat_vel, prev_object); if (!lat_offset_to_avoid) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -808,9 +808,9 @@ TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double signed_time_to_start_collision = [&]() { - const double lon_offset_ego_front_to_obj_back = lon_offset_ego_to_obj_idx + - lat_lon_offset.min_lon_offset - - planner_data_->parameters.front_overhang; + const double lon_offset_ego_front_to_obj_back = + lon_offset_ego_to_obj_idx + lat_lon_offset.min_lon_offset - + (planner_data_->parameters.wheel_base + planner_data_->parameters.front_overhang); const double lon_offset_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx - lat_lon_offset.max_lon_offset - planner_data_->parameters.rear_overhang; @@ -1070,7 +1070,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; } // The ego and object are the opposite directional. - const double obj_acc = -1.0; + const double obj_acc = -0.5; const double decel_time = 1.0; const double obj_moving_dist = (std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 / @@ -1120,9 +1120,10 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); - const double dist_threshold_paths = planner_data_->parameters.vehicle_width / 2.0 + - parameters_->lat_offset_from_obstacle + - calcObstacleMaxLength(obj_shape); + constexpr double dist_threshold_additional_margin = 0.5; + const double dist_threshold_paths = + planner_data_->parameters.vehicle_width / 2.0 + parameters_->lat_offset_from_obstacle + + calcObstacleWidth(obj_shape) / 2.0 + dist_threshold_additional_margin; // crop the ego's path by object position std::vector cropped_ego_path_points; @@ -1204,28 +1205,30 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, - const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left, - const double obj_normal_vel, const std::optional & prev_object) const + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const { - const bool enable_lowpass_filter = true; - /* const bool enable_lowpass_filter = [&]() { - if (!prev_ref_path_points_for_obj_poly_ || prev_ref_path_points_for_obj_poly_->size() < 2) { + if ( + !prev_object || prev_object->ref_path_points_for_obj_poly.size() < 2 || + ref_path_points_for_obj_poly.size() < 2) { return true; } - const size_t prev_front_seg_idx = motion_utils::findNearestSegmentIndex( - *prev_ref_path_points_for_obj_poly_, - ref_path_points_for_obj_poly.front().point.pose.position); constexpr double - min_lane_change_path_lat_offset = 1.0; if ( motion_utils::calcLateralOffset( - *prev_ref_path_points_for_obj_poly_, - ref_path_points_for_obj_poly.front().point.pose.position, prev_front_seg_idx) < - min_lane_change_path_lat_offset) { return true; + const size_t obj_point_idx = + motion_utils::findNearestIndex(ref_path_points_for_obj_poly, obj_pos); + const double paths_lat_diff = std::abs(motion_utils::calcLateralOffset( + prev_object->ref_path_points_for_obj_poly, + ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position)); + + constexpr double min_paths_lat_diff = 0.3; + if (paths_lat_diff < min_paths_lat_diff) { + return true; } // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to // shift the obstacle polygon suddenly. return false; }(); - */ // calculate min/max lateral offset from object to path const auto obj_lat_abs_offset = [&]() { diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index f777e49e5fd78..6bd2f8b9c79e4 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -155,6 +155,7 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio | 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 | ## **Goal Search** diff --git a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml index 0ab617a1a7ab9..6f51b906c4c8f 100644 --- a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -38,6 +38,7 @@ object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 th_moving_object_velocity: 1.0 + detection_bound_offset: 15.0 # pull over pull_over: 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 e90162c958be5..51638e5485fe2 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 @@ -230,6 +230,13 @@ struct LastApprovalData Pose pose{}; }; +enum class DecidingPathStatus { + NOT_DECIDED, + DECIDING, + DECIDED, +}; +using DecidingPathStatusWithStamp = std::pair; + struct PreviousPullOverData { struct SafetyStatus @@ -240,18 +247,37 @@ struct PreviousPullOverData void reset() { - stop_path = nullptr; - stop_path_after_approval = nullptr; found_path = false; safety_status = SafetyStatus{}; - has_decided_path = false; + deciding_path_status = DecidingPathStatusWithStamp{}; } - std::shared_ptr stop_path{nullptr}; - std::shared_ptr stop_path_after_approval{nullptr}; bool found_path{false}; SafetyStatus safety_status{}; - bool has_decided_path{false}; + DecidingPathStatusWithStamp deciding_path_status{}; +}; + +// store stop_pose_ pointer with reason string +struct PoseWithString +{ + std::optional * pose; + std::string string; + + explicit PoseWithString(std::optional * shared_pose) : pose(shared_pose), string("") {} + + void set(const Pose & new_pose, const std::string & new_string) + { + *pose = new_pose; + string = new_string; + } + + void set(const std::string & new_string) { string = new_string; } + + void clear() + { + pose->reset(); + string = ""; + } }; class GoalPlannerModule : public SceneModuleInterface @@ -364,7 +390,7 @@ class GoalPlannerModule : public SceneModuleInterface ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; - PreviousPullOverData prev_data_{nullptr}; + PreviousPullOverData prev_data_{}; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -385,6 +411,7 @@ class GoalPlannerModule : public SceneModuleInterface // debug mutable GoalPlannerDebugData debug_data_; + mutable PoseWithString debug_stop_pose_with_info_; // collision check void initializeOccupancyGridMap(); @@ -404,7 +431,7 @@ class GoalPlannerModule : public SceneModuleInterface void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath() const; - PathWithLaneId generateFeasibleStopPath() const; + PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const; void keepStoppedWithCurrentPath(PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -419,6 +446,8 @@ class GoalPlannerModule : public SceneModuleInterface bool needPathUpdate(const double path_update_duration) const; bool isStuck(); bool hasDecidedPath() const; + bool hasNotDecidedPath() const; + DecidingPathStatusWithStamp checkDecidingPathStatus() const; void decideVelocity(); bool foundPullOverPath() const; void updateStatus(const BehaviorModuleOutput & output); @@ -453,25 +482,18 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output) const; - void setStopPath(BehaviorModuleOutput & output) const; - void updatePreviousData(const BehaviorModuleOutput & output); + void updatePreviousData(); - /** - * @brief Sets a stop path in the current path based on safety conditions and previous paths. - * - * This function sets a stop path in the current path. Depending on whether the previous safety - * judgement against dynamic objects were safe or if a previous stop path existed, it either - * generates a new stop path or uses the previous stop path. - * - * @param output BehaviorModuleOutput - */ - void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const; void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; // new turn signal TurnSignalInfo calcTurnSignalInfo() const; + std::optional last_previous_module_output_{}; + bool hasPreviousModulePathShapeChanged() const; + bool hasDeviatedFromLastPreviousModulePath() const; + // timer for generating pull over path candidates in a separate thread void onTimer(); void onFreespaceParkingTimer(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 21eb22046bff6..2310c0c4d422c 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -74,6 +74,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_margin{0.0}; double object_recognition_collision_check_max_extra_stopping_margin{0.0}; double th_moving_object_velocity{0.0}; + double detection_bound_offset{0.0}; // pull over general params double pull_over_minimum_request_length{0.0}; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index 94cf7bc7ff5dd..e487d9d2ae0b2 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -35,8 +35,13 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; + + // todo(kosuke55):　Functions for this specific use should not be in the interface, + // so it is better to consider interface design when we implement other goal searchers. GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates) const override; + bool isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const override; private: void countObjectsToAvoid( diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp index 93472ab6c0703..2ddacc0aac46d 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -60,6 +60,8 @@ class GoalSearcherBase virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } virtual GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates) const = 0; + virtual bool isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const = 0; protected: GoalPlannerParameters parameters_{}; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ddd7c93998654..9700c44445a65 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -17,7 +17,6 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include #include @@ -114,11 +113,16 @@ class PullOverPlannerBase PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } virtual ~PullOverPlannerBase() = default; + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + void setPlannerData(const std::shared_ptr planner_data) { planner_data_ = planner_data; @@ -132,6 +136,8 @@ class PullOverPlannerBase vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; + + BehaviorModuleOutput previous_module_output_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index bd19dc2e87de0..892ef7d5dd303 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -43,6 +43,8 @@ class ShiftPullOver : public PullOverPlannerBase protected: PathWithLaneId generateReferencePath( const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + std::optional cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index a7aec66f64363..be56d9dc30196 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -45,12 +45,35 @@ using visualization_msgs::msg::MarkerArray; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset); +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh); + +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path); +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_thresh); -bool isAllowedGoalModification(const std::shared_ptr & route_handler); -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose); +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const double extend_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const Pose & extend_pose); // debug MarkerArray createPullOverAreaMarkerArray( 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 493e7ec57f063..faa9e28e28b3b 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 @@ -15,7 +15,6 @@ #include "behavior_path_goal_planner_module/goal_planner_module.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -62,7 +61,8 @@ GoalPlannerModule::GoalPlannerModule( : 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()}, - thread_safe_data_{mutex_, clock_} + thread_safe_data_{mutex_, clock_}, + debug_stop_pose_with_info_{&stop_pose_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -94,7 +94,7 @@ GoalPlannerModule::GoalPlannerModule( // set selected goal searcher // currently there is only one goal_searcher_type const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info); + vehicle_footprint_ = vehicle_info.createFootprint(); goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_, occupancy_grid_map_); @@ -137,11 +137,34 @@ void GoalPlannerModule::updateOccupancyGrid() occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } +bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const +{ + if (!last_previous_module_output_) { + return true; + } + + const auto current_path = getPreviousModuleOutput().path; + + // the terminal distance is far + return calcDistance2d( + last_previous_module_output_->path.points.back().point, + current_path.points.back().point) > 0.3; +} + +bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const +{ + if (!last_previous_module_output_) { + return true; + } + return std::abs(motion_utils::calcLateralOffset( + last_previous_module_output_->path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { - // already generated pull over candidate paths - if (!thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -150,16 +173,30 @@ void GoalPlannerModule::onTimer() return; } - if ( - !planner_data_ || - !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!planner_data_ || !utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } - if (getCurrentStatus() == ModuleStatus::IDLE) { + // check if new pull over path candidates are needed to be generated + const bool need_update = std::invoke([&]() { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + return true; + } + if (hasPreviousModulePathShapeChanged()) { + RCLCPP_ERROR(getLogger(), "has previous module path shape changed"); + return true; + } + if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { + RCLCPP_ERROR(getLogger(), "has deviated from last previous module path"); + return true; + } + return false; + }); + if (!need_update) { return; } + const auto previous_module_output = getPreviousModuleOutput(); const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose @@ -174,8 +211,9 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { planner->setPlannerData(planner_data_); + planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path && isCrossingPossible(*pull_over_path)) { + if (pull_over_path) { pull_over_path->goal_id = goal_candidate.id; pull_over_path->id = path_candidates.size(); path_candidates.push_back(*pull_over_path); @@ -189,9 +227,21 @@ void GoalPlannerModule::onTimer() } } }; + + // todo: currently non centerline input path is supported only by shift pull over + const bool is_center_line_input_path = goal_planner_utils::isReferencePath( + previous_module_output.reference_path, previous_module_output.path, 0.1); + RCLCPP_DEBUG( + getLogger(), "the input path of pull over planner is center line: %d", + is_center_line_input_path); + // plan candidate paths and set them to the member variable if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } @@ -199,6 +249,10 @@ void GoalPlannerModule::onTimer() } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } planCandidatePaths(planner, goal_candidate); } } @@ -214,11 +268,18 @@ void GoalPlannerModule::onTimer() const std::lock_guard lock(mutex_); thread_safe_data_.set_pull_over_path_candidates(path_candidates); thread_safe_data_.set_closest_start_pose(closest_start_pose); + RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); } + + last_previous_module_output_ = previous_module_output; } void GoalPlannerModule::onFreespaceParkingTimer() { + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + if (!planner_data_) { return; } @@ -226,7 +287,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } // fixed goal planner do not use freespace planner - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } @@ -355,7 +416,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const lanelet::ConstLanelets current_lanes = + utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); const bool goal_is_in_current_lanes = std::any_of( current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { @@ -385,14 +448,14 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (!utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + const double request_length = utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { @@ -400,13 +463,6 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } - // if goal modification is not allowed - // 1) goal_pose is in current_lanes, plan path to the original fixed goal - // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { - return goal_is_in_current_lanes; - } - // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes @@ -441,8 +497,14 @@ double GoalPlannerModule::calcModuleRequestLength() const return parameters_->pull_over_minimum_request_length; } - const double minimum_request_length = - *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; + // The module is requested at a distance such that the ego can stop for the pull over start point + // closest to ego. When path planning, each start point is checked to see if it is possible to + // stop again. At that time, if the speed has changed over time, the path will be rejected if + // min_stop_distance is used as is, so scale is applied to provide a buffer. + constexpr double scale_factor_for_buffer = 1.2; + const double minimum_request_length = *min_stop_distance * scale_factor_for_buffer + + parameters_->backward_goal_search_length + + approximate_pull_over_distance_; return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } @@ -586,7 +648,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const { // calculate goal candidates const auto & route_handler = planner_data_->route_handler; - if (goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (utils::isAllowedGoalModification(route_handler)) { return goal_searcher_->search(); } @@ -604,7 +666,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const BehaviorModuleOutput GoalPlannerModule::plan() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(); } @@ -744,7 +806,9 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const if (!thread_safe_data_.foundPullOverPath()) { // situation : not safe against static objects use stop_path - setStopPath(output); + output.path = generateStopPath(); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); setDrivableAreaInfo(output); return; } @@ -754,7 +818,11 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints - setStopPathFromCurrentPath(output); + output.path = + generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path"); + debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); } else { // situation : (safe against static and dynamic objects) or (safe against static objects and // before approval) don't stop @@ -774,50 +842,6 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const -{ - if (prev_data_.found_path || !prev_data_.stop_path) { - // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = generateStopPath(); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); - } else { - // not_safe -> not_safe: use previous stop path - output.path = *prev_data_.stop_path; - // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(output.path); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); - } -} - -void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const -{ - // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) { - auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - const auto stop_path = - behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, - parameters_->maximum_jerk_for_stop); - if (stop_path) { - output.path = *stop_path; - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); - } else { - output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "Collision detected, no feasible stop path found, cannot stop."); - } - } else { - // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = *prev_data_.stop_path_after_approval; - // stop_pose_ is removed in manager every loop, so need to set every loop. - stop_pose_ = utils::getFirstStopPoseFromPath(output.path); - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); - } -} - void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { @@ -879,36 +903,108 @@ void GoalPlannerModule::updateSteeringFactor( bool GoalPlannerModule::hasDecidedPath() const { + return checkDecidingPathStatus().first == DecidingPathStatus::DECIDED; +} + +bool GoalPlannerModule::hasNotDecidedPath() const +{ + return checkDecidingPathStatus().first == DecidingPathStatus::NOT_DECIDED; +} + +DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const +{ + const auto & prev_status = prev_data_.deciding_path_status; + // Once this function returns true, it will continue to return true thereafter - if (prev_data_.has_decided_path) { - return true; + if (prev_status.first == DecidingPathStatus::DECIDED) { + return prev_status; } // if path is not safe, not decided if (!thread_safe_data_.foundPullOverPath()) { - return false; + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } - // If it is dangerous before approval, do not determine the path. + // 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()) { - return false; + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if object recognition for path collision check is enabled, transition to DECIDED after + // DECIDING for a certain period of time if there is no collision. + const auto pull_over_path = thread_safe_data_.get_pull_over_path(); + const auto current_path = pull_over_path->getCurrentPath(); + if (prev_status.first == DecidingPathStatus::DECIDING) { + const double hysteresis_factor = 0.9; + + // check goal pose collision + goal_searcher_->setPlannerData(planner_data_); + const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose(); + if ( + modified_goal_opt && !goal_searcher_->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor)) { + RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // check current parking path collision + 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)) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with 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(); + if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG( + getLogger(), "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); + return {DecidingPathStatus::DECIDED, clock_->now()}; + } + + // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + getLogger(), "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); + return prev_status; } // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data_->self_odometry->pose.pose; - const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position); + const size_t ego_segment_idx = + motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, - thread_safe_data_.get_pull_over_path()->start_pose.position); + const size_t start_pose_segment_idx = + motion_utils::findNearestSegmentIndex(current_path.points, pull_over_path->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, - ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, - start_pose_segment_idx); - return dist_to_parking_start_pose < parameters_->decide_path_distance; + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path->start_pose.position, start_pose_segment_idx); + if (dist_to_parking_start_pose > parameters_->decide_path_distance) { + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if object recognition for path collision check is enabled, transition to DECIDING to check + // collision for a certain period of time. Otherwise, transition to DECIDED directly. + if (parameters_->use_object_recognition) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + return {DecidingPathStatus::DECIDING, clock_->now()}; + } + RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDED"); + return {DecidingPathStatus::DECIDED, clock_->now()}; } void GoalPlannerModule::decideVelocity() @@ -957,6 +1053,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() return output; } + setDebugData(); + return output; } @@ -967,10 +1065,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() return getPreviousModuleOutput(); } - if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { + if (hasNotDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ + RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); thread_safe_data_.clearPullOverPath(); @@ -1024,7 +1123,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updatePreviousData(output); + updatePreviousData(); return output; } @@ -1051,17 +1150,13 @@ void GoalPlannerModule::postProcess() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) +void GoalPlannerModule::updatePreviousData() { - if (prev_data_.found_path || !prev_data_.stop_path) { - prev_data_.stop_path = std::make_shared(output.path); - } - // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. prev_data_.found_path = thread_safe_data_.foundPullOverPath(); - prev_data_.has_decided_path = hasDecidedPath(); + prev_data_.deciding_path_status = checkDecidingPathStatus(); // This is related to safety check, so if it is disabled, return here. if (!parameters_->safety_check_params.enable_safety_check) { @@ -1080,22 +1175,11 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) prev_data_.safety_status.safe_start_time = std::nullopt; } prev_data_.safety_status.is_safe = is_safe; - - // If safety check is enabled, save current path as stop_path_after_approval - // This path is set only once after approval. - if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) { - return; - } - auto stop_path = std::make_shared(output.path); - for (auto & point : stop_path->points) { - point.point.longitudinal_velocity_mps = 0.0; - } - prev_data_.stop_path_after_approval = stop_path; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(); } @@ -1154,11 +1238,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return PathWithLaneId{}; } - // generate reference path - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto reference_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + // extend previous module path to generate reference path for stop path + const auto reference_path = std::invoke([&]() -> PathWithLaneId { + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; + const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); + const double s_end = s_current + common_parameters.forward_path_length; + return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + }); + const auto extended_prev_path = goal_planner_utils::extendPath( + getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length); // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible @@ -1166,7 +1254,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); const auto decel_pose = calcLongitudinalOffsetPose( - reference_path.points, closest_goal_candidate.goal_pose.position, + extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); // if not approved stop road lane. @@ -1177,87 +1265,85 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto stop_pose = std::invoke([&]() -> std::optional { - if (thread_safe_data_.foundPullOverPath()) { - return thread_safe_data_.get_pull_over_path()->start_pose; - } - if (thread_safe_data_.get_closest_start_pose()) { - return thread_safe_data_.get_closest_start_pose().value(); - } - if (!decel_pose) { - return std::nullopt; - } - return decel_pose.value(); - }); - if (!stop_pose) { - return generateFeasibleStopPath(); + const auto stop_pose_with_info = + std::invoke([&]() -> std::optional> { + if (thread_safe_data_.foundPullOverPath()) { + return std::make_pair( + thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose"); + } + if (thread_safe_data_.get_closest_start_pose()) { + return std::make_pair( + thread_safe_data_.get_closest_start_pose().value(), "stop at closest start pose"); + } + if (!decel_pose) { + return std::nullopt; + } + return std::make_pair(decel_pose.value(), "stop at search start pose"); + }); + if (!stop_pose_with_info) { + const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); + // override stop pose info debug string + debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose")); + return feasible_stop_path; } + const Pose stop_pose = stop_pose_with_info->first; // if stop pose is closer than min_stop_distance, stop as soon as possible - const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, *stop_pose); + const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) { - return generateFeasibleStopPath(); + const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); + debug_stop_pose_with_info_.set( + std::string("feasible stop: stop pose is closer than min_stop_distance")); + return feasible_stop_path; } // slow down for turn signal, insert stop point to stop_pose - decelerateForTurnSignal(*stop_pose, reference_path); - stop_pose_ = *stop_pose; // for debug wall marker + auto stop_path = extended_prev_path; + decelerateForTurnSignal(stop_pose, stop_path); + debug_stop_pose_with_info_.set(stop_pose, stop_pose_with_info->second); // slow down before the search area. if (decel_pose) { - decelerateBeforeSearchStart(*decel_pose, reference_path); - return reference_path; + decelerateBeforeSearchStart(*decel_pose, stop_path); + return stop_path; } - // if already passed the decel pose, set pull_over_velocity to reference_path. + // if already passed the decel pose, set pull_over_velocity to stop_path. const auto min_decel_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, pull_over_velocity); - for (auto & p : reference_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); + for (auto & p : stop_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(stop_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { continue; } p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); } - return reference_path; + return stop_path; } -PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const +PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const { - const auto & route_handler = planner_data_->route_handler; - const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & common_parameters = planner_data_->parameters; - - // generate stop reference path - const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); - - const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; - const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); - // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return stop_path; + return path; } // set stop point + auto stop_path = path; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { - stop_pose_ = stop_path.points.at(*stop_idx).point.pose; + debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop"); } return stop_path; @@ -1435,15 +1521,12 @@ bool GoalPlannerModule::checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, const bool update_debug_data) const { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes, - utils::path_safety_checker::isPolygonOverlapLanelet); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_->th_moving_object_velocity); + 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); + std::vector obj_polygons; for (const auto & object : pull_over_lane_stop_objects.objects) { obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); @@ -1882,7 +1965,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1895,6 +1978,14 @@ void GoalPlannerModule::setDebugData() add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } + // Visualize previous module output + add(createPathMarkerArray( + getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); + if (last_previous_module_output_.has_value()) { + add(createPathMarkerArray( + last_previous_module_output_.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); + } + // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( @@ -1958,6 +2049,13 @@ void GoalPlannerModule::setDebugData() } add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); + + // set objects of interest + for (const auto & [uuid, data] : goal_planner_data_.collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); // visualize safety status maker @@ -2016,6 +2114,20 @@ void GoalPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } + + // Visualize stop pose info + if (debug_stop_pose_with_info_.pose->has_value()) { + visualization_msgs::msg::MarkerArray stop_pose_marker_array{}; + const auto color = createMarkerColor(1.0, 1.0, 1.0, 0.99); + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "stop_pose_info", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.5), color); + marker.pose = debug_stop_pose_with_info_.pose->value(); + marker.text = debug_stop_pose_with_info_.string; + stop_pose_marker_array.markers.push_back(marker); + add(stop_pose_marker_array); + add(createPoseMarkerArray(marker.pose, "stop_pose", 1.0, 1.0, 1.0, 0.9)); + } } void GoalPlannerModule::printParkingPositionError() const diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index bf359be3de18b..e2d12dd82cbe4 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -170,11 +170,13 @@ GoalCandidates GoalSearcher::search() transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { - continue; + // break here to exclude goals located laterally in no_parking_areas + break; } if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { - continue; + // break here to exclude goals located laterally in no_stopping_areas + break; } if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { @@ -264,14 +266,11 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { - const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); - const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + 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); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); @@ -313,6 +312,43 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } } +// Note: this function is not just return goal_candidate.is_safe but check collision with +// current planner_data_ and margin scale factor. +// And is_safe is not updated in this function. +bool GoalSearcher::isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const +{ + if (!parameters_.use_object_recognition) { + return true; + } + + const Pose goal_pose = goal_candidate.goal_pose; + + 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 double margin = parameters_.object_recognition_collision_check_margin * margin_scale_factor; + + if (utils::checkCollisionBetweenFootprintAndObjects( + vehicle_footprint_, goal_pose, pull_over_lane_stop_objects, margin)) { + return false; + } + + // check longitudinal margin with pull over lane objects + constexpr bool filter_inside = true; + const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, margin, + filter_inside); + if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { + return false; + } + + return true; +} + bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const { if (parameters_.use_occupancy_grid_for_goal_search) { diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 3079361253c31..c6fe97874f012 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -101,6 +101,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.detection_bound_offset = node->declare_parameter(ns + "detection_bound_offset"); } // pull over general params @@ -419,7 +420,7 @@ bool GoalPlannerModuleManager::isAlwaysExecutableModule() const { // enable AlwaysExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -434,7 +435,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -449,7 +450,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 712da5aa03a4e..b168da61e6239 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -46,10 +46,10 @@ std::optional ShiftPullOver::plan(const Pose & goal_pose) const int shift_sampling_num = parameters_.shift_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; - // get road and shoulder lanes - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_search_length, forward_search_length, + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + previous_module_output_.path, planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, backward_search_length, forward_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { @@ -99,6 +99,31 @@ PathWithLaneId ShiftPullOver::generateReferencePath( return road_lane_reference_path; } +std::optional ShiftPullOver::cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const +{ + // clip previous module path to shift end pose nearest segment index + const size_t shift_end_idx = + motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position); + std::vector clipped_points{ + prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected shift end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength( + prev_module_path.points, shift_end_idx, shift_end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_prev_module_path = prev_module_path; + clipped_prev_module_path.points = clipped_points; + + return clipped_prev_module_path; +} + std::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -106,33 +131,55 @@ std::optional ShiftPullOver::generatePullOverPath( const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); - // generate road lane reference path to shift end + // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); + const auto prev_module_path = utils::resamplePathWithSpline( + previous_module_output_.path, parameters_.center_line_path_interval); + const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; + + // process previous module path for path shifter input path + // case1) extend path if shift end pose is behind of previous module path terminal pose + // case2) crop path if shift end pose is ahead of previous module path terminal pose + const auto processed_prev_module_path = std::invoke([&]() -> std::optional { + const bool extend_previous_module_path = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > + lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; + if (extend_previous_module_path) { // case1 + return goal_planner_utils::extendPath( + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose); + } else { // case2 + return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); + } + }); + if (!processed_prev_module_path || processed_prev_module_path->points.empty()) { + return {}; + } // calculate shift length - const Pose & shift_end_pose_road_lane = - road_lane_reference_path_to_shift_end.points.back().point.pose; + const Pose & shift_end_pose_prev_module_path = + processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + tier4_autoware_utils::inverseTransformPoint( + shift_end_pose.position, shift_end_pose_prev_module_path) .y; // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( - road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( - road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + processed_prev_module_path->points, shift_end_pose_prev_module_path.position, -before_shifted_pull_over_distance); - if (!shift_start_pose) return {}; // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(road_lane_reference_path_to_shift_end); + path_shifter.setPath(processed_prev_module_path.value()); ShiftLine shift_line{}; shift_line.start = *shift_start_pose; shift_line.end = shift_end_pose; @@ -140,7 +187,9 @@ std::optional ShiftPullOver::generatePullOverPath( path_shifter.addShiftLine(shift_line); ShiftedPath shifted_path{}; const bool offset_back = true; // offset front side from reference path - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + if (!path_shifter.generate(&shifted_path, offset_back)) { + return {}; + } shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); motion_utils::insertOrientation(shifted_path.path.points, true); @@ -208,8 +257,13 @@ std::optional ShiftPullOver::generatePullOverPath( pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; - pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); + pull_over_path.debug_poses.push_back(goal_pose); + pull_over_path.debug_poses.push_back(shift_end_pose); + pull_over_path.debug_poses.push_back( + road_lane_reference_path_to_shift_end.points.back().point.pose); + pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); // check if the parking path will leave lanes const auto drivable_lanes = @@ -218,8 +272,10 @@ std::optional ShiftPullOver::generatePullOverPath( const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 1a1e66f85b403..8ad90d26f58f2 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -14,6 +14,7 @@ #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -41,6 +42,7 @@ using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; + lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance) @@ -74,6 +76,41 @@ lanelet::ConstLanelets getPullOverLanes( outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset) +{ + const auto pull_over_lanes = + getPullOverLanes(route_handler, left_side, backward_distance, forward_distance); + + return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0) + : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset); +} + +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects) +{ + const auto lanes = generateExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset); + + const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( + objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + + return objects_in_lanes; +} + +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh) +{ + const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset, objects); + + return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh); +} + PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside) @@ -196,22 +233,138 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification(const std::shared_ptr & route_handler) +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path) { - return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); + double lateral_deviation = 0.0; + for (const auto & target_point : target_path.points) { + const size_t nearest_index = + motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + lateral_deviation = std::max( + lateral_deviation, + std::abs(tier4_autoware_utils::calcLateralDeviation( + reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); + } + return lateral_deviation; } -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_threshold) { - const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const auto shoulder_lanes = route_handler->getShoulderLanelets(); + return calcLateralDeviationBetweenPaths(reference_path, target_path) < + lateral_deviation_threshold; +} - lanelet::ConstLanelet closest_shoulder_lane{}; - if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { - return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) +{ + const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + std::vector clipped_points{ + path.points.begin(), path.points.begin() + end_idx}; + if (clipped_points.empty()) { + return std::nullopt; } - return false; + // add projected end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_path = path; + clipped_path.points = clipped_points; + + return clipped_path; +} + +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length) +{ + const auto & points = path.points; + + double sum_length = 0; + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length + seg_length) { + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.begin() + i}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + + // add precise end pose to cropped points + const double remaining_length = forward_length - sum_length; + const Pose precise_end_pose = + calcOffsetPose(cropped_path.points.back().point.pose, remaining_length, 0, 0); + if (remaining_length < 0.1) { + // if precise_end_pose is too close, replace the last point + cropped_path.points.back().point.pose = precise_end_pose; + } else { + auto precise_end_point = cropped_path.points.back(); + precise_end_point.point.pose = precise_end_pose; + cropped_path.points.push_back(precise_end_point); + } + return cropped_path; + } + sum_length += seg_length; + } + + // if forward_length is too long, return points after target_seg_idx + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.end()}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + return cropped_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + + // generate clipped road lane reference path from previous module path terminal pose to shift end + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + + PathWithLaneId clipped_path = + cropForwardPoints(reference_path, target_path_terminal_idx, extend_length); + + // shift clipped path to previous module path terminal pose + const double lateral_shift_from_reference_path = + motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + for (auto & p : clipped_path.points) { + p.point.pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); + } + + auto extended_path = target_path; + const auto start_point = + std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { + const bool is_forward = + tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x > + 0.0; + const bool is_close = tier4_autoware_utils::calcDistance2d( + p.point.pose.position, target_terminal_pose.position) < 0.1; + return is_forward && !is_close; + }); + std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); + + extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + + return extended_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + const double extend_distance = motion_utils::calcSignedArcLength( + reference_path.points, target_path_terminal_idx, extend_pose.position); + + return extendPath(target_path, reference_path, extend_distance); } } // namespace behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp index b34f691bd2b0d..31561b6210591 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp @@ -45,6 +45,8 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; protected: + void initParams(rclcpp::Node * node); + std::shared_ptr parameters_; Direction direction_; diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index e4405a31a360d..0a56c17d89fd0 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -29,10 +29,14 @@ namespace behavior_path_planner void LaneChangeModuleManager::init(rclcpp::Node * node) { - using tier4_autoware_utils::getOrDeclareParameter; - // init manager interface initInterface(node, {""}); + initParams(node); +} + +void LaneChangeModuleManager::initParams(rclcpp::Node * node) +{ + using tier4_autoware_utils::getOrDeclareParameter; LaneChangeParameters p{}; 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 91e34c0e91931..439e08fd7e94f 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -67,7 +67,7 @@ priority: 4 max_module_size: 1 - avoidance_by_lc: + avoidance_by_lane_change: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index f6f95ae3b5e82..5e773eba96aee 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -46,6 +46,7 @@ using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = tier4_autoware_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; +using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; enum Action { ADD = 0, @@ -456,6 +457,8 @@ class PlannerManager std::unique_ptr debug_publisher_ptr_; + std::unique_ptr state_publisher_ptr_; + pluginlib::ClassLoader plugin_loader_; mutable rclcpp::Logger logger_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9b3fbedc37203..3eb5eca5954fb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -784,6 +784,7 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSh { std::lock_guard lock(mutex_pd_); + planner_data_->traffic_light_id_map.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 262de4764fcfe..cbcec2e3095d3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -40,6 +40,7 @@ PlannerManager::PlannerManager( { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); + state_publisher_ptr_ = std::make_unique(&node, "~/debug"); } void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name) @@ -859,11 +860,9 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) // when lane change module is running, don't update root lanelet. const bool is_lane_change_running = std::invoke([&]() { - const auto lane_change_itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { - return m->name().find("lane_change") != std::string::npos || - m->name().find("avoidance_by_lc") != std::string::npos; - }); + const auto lane_change_itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->isRootLaneletToBeUpdated(); }); return lane_change_itr != approved_module_ptrs_.end(); }); if (is_lane_change_running) { @@ -910,10 +909,6 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) void PlannerManager::print() const { - if (!verbose_) { - return; - } - const auto get_status = [](const auto & m) { return magic_enum::enum_name(m->getCurrentStatus()); }; @@ -963,6 +958,12 @@ void PlannerManager::print() const << std::setw(21); } + state_publisher_ptr_->publish("internal_state", string_stream.str()); + + if (!verbose_) { + return; + } + RCLCPP_INFO_STREAM(logger_, string_stream.str()); } diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 43ecd17fa1844..e8a28ee684ce6 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -268,7 +268,14 @@ class SceneModuleManagerInterface // init manager configuration { std::string ns = name_ + "."; - config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + try { + config_.enable_rtc = getOrDeclareParameter(*node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(*node, ns + "enable_rtc"); + } catch (const std::exception & e) { + config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + } + config_.enable_simultaneous_execution_as_approved_module = getOrDeclareParameter(*node, ns + "enable_simultaneous_execution_as_approved_module"); config_.enable_simultaneous_execution_as_candidate_module = getOrDeclareParameter( diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp deleted file mode 100644 index edb6d32bda205..0000000000000 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ - -#include -#include - -inline tier4_autoware_utils::LinearRing2d createVehicleFootprint( - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) -{ - using tier4_autoware_utils::LinearRing2d; - using tier4_autoware_utils::Point2d; - - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m + margin; - const double x_center = i.wheel_base_m / 2.0; - const double x_rear = -(i.rear_overhang_m + margin); - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; - const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); - - LinearRing2d footprint; - footprint.push_back(Point2d{x_front, y_left}); - footprint.push_back(Point2d{x_front, y_right}); - footprint.push_back(Point2d{x_center, y_right}); - footprint.push_back(Point2d{x_rear, y_right}); - footprint.push_back(Point2d{x_rear, y_left}); - footprint.push_back(Point2d{x_center, y_left}); - footprint.push_back(Point2d{x_front, y_left}); - - return footprint; -} - -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 3dc230f0e92ed..74f6b843803df 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -38,11 +38,6 @@ struct StartGoalPlannerData std::vector ego_predicted_path; // collision check debug map CollisionCheckDebugMap collision_check; - - Pose refined_start_pose; - std::vector start_pose_candidates; - size_t selected_start_pose_candidate_index; - double margin_for_start_pose_candidate; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index 01b2880f4362a..dcee7696933dd 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -221,6 +221,9 @@ PathWithLaneId refinePathForGoal( bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); + bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); bool isInLaneletWithYawThreshold( @@ -303,6 +306,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, const double forward_length, const bool forward_only_in_route); +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route); + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length, diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 44358dfa84e4e..6ddc0df4eebf4 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -415,15 +415,7 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const double dist_to_original_desired_start = get_distance(original_desired_start_point) - base_link2front_; - const double dist_to_original_desired_end = get_distance(original_desired_end_point); - const double dist_to_original_required_start = - get_distance(original_required_start_point) - base_link2front_; - const double dist_to_original_required_end = get_distance(original_required_end_point); const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; - const double dist_to_new_desired_end = get_distance(new_desired_end_point); - const double dist_to_new_required_start = - get_distance(new_required_start_point) - base_link2front_; - const double dist_to_new_required_end = get_distance(new_required_end_point); // If we still do not reach the desired front point we ignore it if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) { @@ -435,6 +427,9 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_end = get_distance(new_desired_end_point); + // If we already passed the desired end point, return the other signal if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) { TurnSignalInfo empty_signal_info; @@ -445,6 +440,13 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_required_start = + get_distance(original_required_start_point) - base_link2front_; + const double dist_to_original_required_end = get_distance(original_required_end_point); + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + const double dist_to_new_required_end = get_distance(new_required_end_point); + if (dist_to_original_desired_start <= dist_to_new_desired_start) { const auto enable_prior = use_prior_turn_signal( dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start, 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 44c3025a8c5d5..03fae6864fe50 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 @@ -208,26 +208,25 @@ std::optional> intersectBound( return std::nullopt; } -double calcDistanceFromPointToSegment( +double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_start_point, const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcSquaredDistance2d; + const auto & a = segment_start_point; const auto & b = segment_end_point; const auto & p = target_point; const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); - const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + const double squared_segment_length = calcSquaredDistance2d(a, b); if (0 <= dot_val && dot_val <= squared_segment_length) { - const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); - const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); - return numerator / denominator; + return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length; } // target_point is outside the segment. - return std::min( - tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p)); + return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p)); } PolygonPoint transformBoundFrenetCoordinate( @@ -238,8 +237,8 @@ PolygonPoint transformBoundFrenetCoordinate( // find wrong nearest index. std::vector dist_to_bound_segment_vec; for (size_t i = 0; i < bound_points.size() - 1; ++i) { - const double dist_to_bound_segment = - calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + const double dist_to_bound_segment = calcSquaredDistanceFromPointToSegment( + bound_points.at(i), bound_points.at(i + 1), target_point); dist_to_bound_segment_vec.push_back(dist_to_bound_segment); } diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index bf93e71ab3591..e209e8dba36be 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -453,16 +453,15 @@ bool checkSafetyWithIntegralPredictedPolygon( for (const auto & path : object.predicted_paths) { for (const auto & pose_with_poly : path.path) { if (boost::geometry::overlaps(ego_integral_polygon, pose_with_poly.poly)) { - { - debug_pair.second.ego_predicted_path = ego_predicted_path; // raw path - debug_pair.second.obj_predicted_path = path.path; // raw path - debug_pair.second.extended_obj_polygon = pose_with_poly.poly; - debug_pair.second.extended_ego_polygon = - ego_integral_polygon; // time filtered extended polygon - updateCollisionCheckDebugMap(debug_map, debug_pair, false); - } + debug_pair.second.ego_predicted_path = ego_predicted_path; // raw path + debug_pair.second.obj_predicted_path = path.path; // raw path + debug_pair.second.extended_obj_polygon = pose_with_poly.poly; + debug_pair.second.extended_ego_polygon = + ego_integral_polygon; // time filtered extended polygon + updateCollisionCheckDebugMap(debug_map, debug_pair, /*is_safe=*/false); return false; } + updateCollisionCheckDebugMap(debug_map, debug_pair, /*is_safe=*/true); } } } diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index e6f9ce21cebf7..308cf9c4fed2c 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1450,6 +1450,70 @@ lanelet::ConstLanelets getExtendedCurrentLanes( return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); } +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route) +{ + auto lanes = getCurrentLanesFromPath(path, planner_data); + + if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); + double forward_length_sum = 0.0; + double backward_length_sum = 0.0; + + while (backward_length_sum < backward_length) { + auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front()); + } else { + break; // no more previous lanes to add + } + lanes = extended_lanes; + } + + while (forward_length_sum < forward_length) { + auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back()); + } else { + break; // no more next lanes to add + } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + + lanes = extended_lanes; + } + + return lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) @@ -1551,4 +1615,22 @@ std::string convertToSnakeCase(const std::string & input_str) } return output_str; } + +bool isAllowedGoalModification(const std::shared_ptr & route_handler) +{ + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); +} + +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +{ + const Pose & goal_pose = route_handler->getOriginalGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet closest_shoulder_lane{}; + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } + + return false; +} } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_side_shift_module/README.md b/planning/behavior_path_side_shift_module/README.md index b76e549eb602f..6a460d8423cbd 100644 --- a/planning/behavior_path_side_shift_module/README.md +++ b/planning/behavior_path_side_shift_module/README.md @@ -2,6 +2,29 @@ (For remote control) Shift the path to left or right according to an external instruction. +## Overview of the Side Shift Module Process + +1. Receive the required lateral offset input. +2. Update the `requested_lateral_offset_` under the following conditions: + a. Verify if the last update time has elapsed. + b. Ensure the required lateral offset value is different from the previous one. +3. Insert the shift points into the path if the side shift module's status is not in the SHIFTING status. + +Please be aware that `requested_lateral_offset_` is continuously updated with the latest values and is not queued. + +## Statuses of the Side Shift + +The side shift has three distinct statuses. Note that during the SHIFTING status, the path cannot be updated: + +1. BEFORE_SHIFT: Preparing for shift. +2. SHIFTING: Currently in the process of shifting. +3. AFTER_SHIFT: Shift completed. + +
+ ![case1](images/side_shift_status.drawio.svg){width=1000} +
side shift status
+
+ ## Flowchart ```plantuml @@ -27,7 +50,7 @@ stop @enduml ``` -```plantuml --> +```plantuml @startuml skinparam monochrome true skinparam defaultTextAlignment center diff --git a/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg b/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg new file mode 100644 index 0000000000000..447a00236d2b6 --- /dev/null +++ b/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg @@ -0,0 +1,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + SideShiftStatus + + ::SHIFTING +
+ + +
+
+ + +
+
+ SideShiftStatus + ::BEFORE_SHIFT + +
+ +
+
+ + +
+
SIDE SHIFT START POINT
+
+ + +
+
SIDE SHIFT END POINT
+
+ + +
+
+ SideShiftStatus + ::AFTER_SHIFT + +
+ +
+
+ + diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index aeefd18357a5c..b26864eb450d5 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -16,7 +16,7 @@ Use cases include: - pull out from the shoulder lane to the road lane centerline.
- ![case2](images/start_from_start_from_road_shoulder.drawio.svg){width=1000} + ![case2](images/start_from_road_shoulder.drawio.svg){width=1000}
pull out from the shoulder lane
diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp similarity index 80% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 66dbddebf99bb..8f8d2baf9c85e 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -29,10 +29,30 @@ namespace behavior_path_planner { +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStarParam; +struct StartPlannerDebugData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; + + Pose refined_start_pose; + std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; +}; + struct StartPlannerParameters { double th_arrived_distance{0.0}; @@ -105,4 +125,4 @@ struct StartPlannerParameters } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp new file mode 100644 index 0000000000000..6aa5584807d90 --- /dev/null +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp @@ -0,0 +1,39 @@ +// 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. + +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ + +#include "behavior_path_start_planner_module/data_structs.hpp" + +#include +#include + +namespace behavior_path_planner +{ +using behavior_path_planner::StartPlannerDebugData; + +void updateSafetyCheckDebugData( + StartPlannerDebugData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index de3d376fa4b3d..0144fdd45ae50 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -16,9 +16,8 @@ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -45,7 +44,7 @@ class PullOutPlannerBase explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } virtual ~PullOutPlannerBase() = default; 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 2d50ae189dc13..fef9a4aa8ebfc 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,6 +50,11 @@ 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) + { + drivable_lanes_ = drivable_lanes; + } + std::shared_ptr lane_departure_checker_; private: @@ -58,6 +63,8 @@ class ShiftPullOut : public PullOutPlannerBase double calcPullOutLongitudinalDistance( 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_; }; } // 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 41a80e59d56bf..5bbde1c2fc523 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 @@ -21,11 +21,11 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/freespace_pull_out.hpp" #include "behavior_path_start_planner_module/geometric_pull_out.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/shift_pull_out.hpp" -#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -191,7 +191,7 @@ class StartPlannerModule : public SceneModuleInterface std::vector> start_planners_; PullOutStatus status_; - mutable StartGoalPlannerData start_planner_data_; + mutable StartPlannerDebugData debug_data_; std::deque odometry_buffer_; @@ -242,6 +242,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; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; @@ -254,7 +256,7 @@ class StartPlannerModule : public SceneModuleInterface void onFreespacePlannerTimer(); bool planFreespacePath(); - void setDebugData() const; + void setDebugData(); void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/src/debug.cpp b/planning/behavior_path_start_planner_module/src/debug.cpp new file mode 100644 index 0000000000000..0f95470d86d40 --- /dev/null +++ b/planning/behavior_path_start_planner_module/src/debug.cpp @@ -0,0 +1,30 @@ +// 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 "behavior_path_start_planner_module/debug.hpp" + +namespace behavior_path_planner +{ + +void updateSafetyCheckDebugData( + StartPlannerDebugData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +} // namespace behavior_path_planner 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 51673410199b8..3b4d08b65923c 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 @@ -49,11 +49,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - if (pull_out_lanes.empty()) { - return std::nullopt; - } - const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); @@ -69,41 +64,17 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos pull_out_path.partial_paths.front(); // shift path is not separate but only one. // check lane_departure with path between pull_out_start to pull_out_end - PathWithLaneId path_start_to_end{}; + PathWithLaneId path_shift_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); + const size_t pull_out_end_idx = + findNearestIndex(shift_path.points, pull_out_path.end_pose.position); - // calculate collision check end idx - const size_t collision_check_end_idx = std::invoke([&]() { - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - shift_path.points, pull_out_path.end_pose.position, - parameters_.collision_check_distance_from_end); - - if (collision_check_end_pose) { - return findNearestIndex(shift_path.points, collision_check_end_pose->position); - } else { - return shift_path.points.size() - 1; - } - }); - path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, - shift_path.points.begin() + collision_check_end_idx + 1); + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + pull_out_end_idx + 1); } - // extract shoulder lanes from pull out lanes - lanelet::ConstLanelets shoulder_lanes; - std::copy_if( - pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), - [&route_handler](const auto & pull_out_lane) { - return route_handler->isShoulderLanelet(pull_out_lane); - }); - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip)); - // crop backward path // removes points which are out of lanes up to the start pose. // this ensures that the backward_path stays within the drivable area when starting from a @@ -117,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(expanded_lanes, transformed_vehicle_footprint); + LaneDepartureChecker::isOutOfLane(drivable_lanes_, transformed_vehicle_footprint); if (i <= start_segment_idx) { if (!is_out_of_lane) { cropped_path.points.push_back(shift_path.points.at(i)); @@ -131,7 +102,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(drivable_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 1183281a9392b..6c9000caac956 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 @@ -14,10 +14,10 @@ #include "behavior_path_start_planner_module/start_planner_module.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_start_planner_module/debug.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -86,6 +86,10 @@ StartPlannerModule::StartPlannerModule( void StartPlannerModule::onFreespacePlannerTimer() { + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + if (!planner_data_) { return; } @@ -96,7 +100,11 @@ void StartPlannerModule::onFreespacePlannerTimer() const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; - if (isStuck() && is_new_costmap) { + if (!is_new_costmap) { + return; + } + + if (isStuck()) { planFreespacePath(); } } @@ -127,7 +135,8 @@ void StartPlannerModule::initVariables() resetPathReference(); debug_marker_.markers.clear(); initializeSafetyCheckParameters(); - initializeCollisionCheckDebugMap(start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(debug_data_.collision_check); + updateDrivableLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -560,6 +569,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; + updateDrivableLanes(); } void StartPlannerModule::incrementPathIndex() @@ -590,8 +600,8 @@ void StartPlannerModule::planWithPriority( if (findPullOutPath( start_pose_candidates[index], planner, refined_start_pose, goal_pose, collision_check_margin)) { - start_planner_data_.selected_start_pose_candidate_index = index; - start_planner_data_.margin_for_start_pose_candidate = collision_check_margin; + debug_data_.selected_start_pose_candidate_index = index; + debug_data_.margin_for_start_pose_candidate = collision_check_margin; return; } } @@ -630,7 +640,7 @@ bool StartPlannerModule::findPullOutPath( const auto & dynamic_objects = planner_data_->dynamic_object; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - const auto & vehicle_footprint = createVehicleFootprint(vehicle_info_); + const auto & vehicle_footprint = vehicle_info_.createFootprint(); // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_->th_moving_object_velocity); @@ -849,8 +859,8 @@ void StartPlannerModule::updatePullOutStatus() start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); } - start_planner_data_.refined_start_pose = *refined_start_pose; - start_planner_data_.start_pose_candidates = start_pose_candidates; + debug_data_.refined_start_pose = *refined_start_pose; + debug_data_.start_pose_candidates = start_pose_candidates; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -907,7 +917,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( { std::vector pull_out_start_pose_candidates{}; const auto start_pose = planner_data_->route_handler->getOriginalStartPose(); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); const double backward_path_length = @@ -1197,14 +1207,13 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - utils::parking_departure::updateSafetyCheckTargetObjectsData( - start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + behavior_path_planner::updateSafetyCheckDebugData( + debug_data_, filtered_objects, target_objects_on_lane, 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, - start_planner_data_.collision_check, planner_data_->parameters, - safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, - hysteresis_factor); + debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, + objects_filtering_params_->use_all_predicted_path, hysteresis_factor); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1326,7 +1335,43 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -void StartPlannerModule::setDebugData() const +void StartPlannerModule::updateDrivableLanes() +{ + const auto drivable_lanes = createDrivableLanes(); + for (auto & planner : start_planners_) { + auto shift_pull_out = std::dynamic_pointer_cast(planner); + + if (shift_pull_out) { + shift_pull_out->setDrivableLanes(drivable_lanes); + } + } +} + +lanelet::ConstLanelets StartPlannerModule::createDrivableLanes() const +{ + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + if (pull_out_lanes.empty()) { + return lanelet::ConstLanelets{}; + } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [this](const auto & pull_out_lane) { + return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); + }); + const auto drivable_lanes = utils::transformToLanelets( + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); + return drivable_lanes; +} + +void StartPlannerModule::setDebugData() { using marker_utils::addFootprintMarker; using marker_utils::createFootprintMarkerArray; @@ -1355,13 +1400,13 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createFootprintMarkerArray( - start_planner_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); + debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0)); // visualize collision_check_end_pose and footprint { - const auto local_footprint = createVehicleFootprint(vehicle_info_); + const auto local_footprint = vehicle_info_.createFootprint(); const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, parameters_->collision_check_distance_from_end); @@ -1400,14 +1445,13 @@ void StartPlannerModule::setDebugData() const visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); - for (size_t i = 0; i < start_planner_data_.start_pose_candidates.size(); ++i) { + for (size_t i = 0; i < debug_data_.start_pose_candidates.size(); ++i) { footprint_marker.id = i; text_marker.id = i; footprint_marker.points.clear(); text_marker.text = "idx[" + std::to_string(i) + "]"; - text_marker.pose = start_planner_data_.start_pose_candidates.at(i); - addFootprintMarker( - footprint_marker, start_planner_data_.start_pose_candidates.at(i), vehicle_info_); + text_marker.pose = debug_data_.start_pose_candidates.at(i); + addFootprintMarker(footprint_marker, debug_data_.start_pose_candidates.at(i), vehicle_info_); start_pose_footprint_marker_array.markers.push_back(footprint_marker); start_pose_text_marker_array.markers.push_back(text_marker); } @@ -1416,24 +1460,64 @@ void StartPlannerModule::setDebugData() const add(start_pose_text_marker_array); } + // 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); + 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(); + { + const size_t pull_out_start_idx = motion_utils::findNearestIndex( + shift_path.points, status_.pull_out_path.start_pose.position); + const size_t pull_out_end_idx = + motion_utils::findNearestIndex(shift_path.points, status_.pull_out_path.end_pose.position); + + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + pull_out_end_idx + 1); + } + + for (size_t i = 0; i < path_shift_start_to_end.points.size(); ++i) { + pull_out_path_footprint_marker.id = i; + pull_out_path_footprint_marker.points.clear(); + addFootprintMarker( + pull_out_path_footprint_marker, path_shift_start_to_end.points.at(i).point.pose, + vehicle_info_); + pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker); + } + + add(pull_out_path_footprint_marker_array); + } + // safety check if (parameters_->safety_check_params.enable_safety_check) { - if (start_planner_data_.ego_predicted_path.size() > 0) { + if (debug_data_.ego_predicted_path.size() > 0) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( - start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add(createPredictedPathMarkerArray( ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9)); } - if (start_planner_data_.filtered_objects.objects.size() > 0) { + if (debug_data_.filtered_objects.objects.size() > 0) { add(createObjectsMarkerArray( - start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } + + add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info")); + add(showPredictedPath(debug_data_.collision_check, "ego_predicted_path")); + add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation")); + + // set objects of interest + for (const auto & [uuid, data] : debug_data_.collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); - add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); - add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); - initializeCollisionCheckDebugMap(start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(debug_data_.collision_check); } // Visualize planner type text diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index 73a1e94399503..b34398083a95c 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -14,7 +14,6 @@ #include "behavior_path_start_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index f9ddd3ce61099..3c22d1fe65db5 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,5 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] + opposite_adjacent_extend_width: 1.5 # [m] enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 7da494bfd5231..5cc90afb0043d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -145,14 +145,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.conflict_areas_for_blind_spot, "conflict_area_for_blind_spot", module_id_, 0.0, - 0.5, 0.5), + debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), &debug_marker_array, now); appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.detection_areas_for_blind_spot, "detection_area_for_blind_spot", module_id_, 0.5, - 0.0, 0.0), + debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0), &debug_marker_array, now); appendMarkerArray( diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index d64f1ebc39a51..d114ab0c65da9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -16,7 +16,6 @@ #include #include -#include #include @@ -28,12 +27,10 @@ namespace behavior_velocity_planner { -using tier4_autoware_utils::getOrDeclareParameter; BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.use_pass_judge_line = @@ -48,6 +45,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + planner_param_.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 62a5e06674d4e..efbff7e2af51d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -105,16 +105,18 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; const auto straight_lanelets = getStraightLanelets(lanelet_map_ptr, routing_graph_ptr, lane_id_); - if (!generateStopLine(straight_lanelets, path, &stop_line_idx)) { + const auto stoplines_idx_opt = generateStopLine(straight_lanelets, path); + if (!stoplines_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "[BlindSpotModule::run] setStopLineIdx fail"); *path = input_path; // reset path return false; } - if (stop_line_idx <= 0) { + const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); + + if (default_stopline_idx <= 0) { RCLCPP_DEBUG( logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); *path = input_path; // reset path @@ -133,6 +135,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return false; } const size_t closest_idx = closest_idx_opt.value(); + const auto stop_line_idx = + closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; /* set judge line dist */ const double current_vel = planner_data_->current_velocity->twist.linear.x; @@ -156,9 +160,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto debug_data_.virtual_wall_pose = stop_line_pose; const auto stop_pose = path->points.at(stop_line_idx).point.pose; debug_data_.stop_point_pose = stop_pose; - auto offset_pose = motion_utils::calcLongitudinalOffsetPose( - path->points, stop_pose.position, -pass_judge_line_dist); - if (offset_pose) debug_data_.judge_point_pose = *offset_pose; /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ if (planner_param_.use_pass_judge_line) { @@ -235,69 +236,41 @@ std::optional BlindSpotModule::getFirstPointConflictingLanelets( } } -bool BlindSpotModule::generateStopLine( +std::optional> BlindSpotModule::generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const { /* set parameters */ constexpr double interval = 0.2; const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interval); - const int base2front_idx_dist = - std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); + // const int base2front_idx_dist = + // std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); /* spline interpolation */ autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; if (!splineInterpolate(*path, interval, path_ip, logger_)) { - return false; + return std::nullopt; } /* generate stop point */ - int stop_idx_ip = 0; // stop point index for interpolated path. + size_t stop_idx_default_ip = 0; // stop point index for interpolated path. + size_t stop_idx_critical_ip = 0; if (straight_lanelets.size() > 0) { std::optional first_idx_conflicting_lane_opt = getFirstPointConflictingLanelets(path_ip, straight_lanelets); if (!first_idx_conflicting_lane_opt) { RCLCPP_DEBUG(logger_, "No conflicting line found."); - return false; - } - stop_idx_ip = std::max( - first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); - } else { - std::optional intersection_enter_point_opt = - getStartPointFromLaneLet(lane_id_); - if (!intersection_enter_point_opt) { - RCLCPP_DEBUG(logger_, "No intersection enter point found."); - return false; - } - - geometry_msgs::msg::Pose intersection_enter_pose; - intersection_enter_pose = intersection_enter_point_opt.value(); - const auto stop_idx_ip_opt = - motion_utils::findNearestIndex(path_ip.points, intersection_enter_pose, 10.0, M_PI_4); - if (stop_idx_ip_opt) { - stop_idx_ip = stop_idx_ip_opt.value(); + return std::nullopt; } - - stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); + stop_idx_default_ip = std::max(first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist, 0); + stop_idx_critical_ip = std::max(first_idx_conflicting_lane_opt.value() - 1, 0); } /* insert stop_point to use interpolated path*/ - *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path); + const size_t stopline_idx_default = insertPoint(stop_idx_default_ip, path_ip, path); + const size_t stopline_idx_critical = insertPoint(stop_idx_critical_ip, path_ip, path); - /* if another stop point exist before intersection stop_line, disable judge_line. */ - bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { - if (std::fabs(path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { - has_prior_stopline = true; - break; - } - } - - RCLCPP_DEBUG( - logger_, "generateStopLine() : stop_idx = %d, stop_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, stop_idx_ip, has_prior_stopline); - - return true; + return std::make_pair(stopline_idx_default, stopline_idx_critical); } void BlindSpotModule::cutPredictPathWithDuration( @@ -385,36 +358,61 @@ bool BlindSpotModule::checkObstacleInBlindSpot( const auto areas_opt = generateBlindSpotPolygons( lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); - if (!!areas_opt) { - debug_data_.detection_areas_for_blind_spot = areas_opt.value().detection_areas; - debug_data_.conflict_areas_for_blind_spot = areas_opt.value().conflict_areas; + if (areas_opt) { + const auto & detection_areas = areas_opt.value().detection_areas; + const auto & conflict_areas = areas_opt.value().conflict_areas; + const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas; + const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas; + debug_data_.detection_areas = detection_areas; + debug_data_.conflict_areas = conflict_areas; + debug_data_.detection_areas.insert( + debug_data_.detection_areas.end(), opposite_detection_areas.begin(), + opposite_detection_areas.end()); + debug_data_.conflict_areas.insert( + debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(), + opposite_conflict_areas.end()); autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); // check objects in blind spot areas - bool obstacle_detected = false; for (const auto & object : objects.objects) { if (!isTargetObjectType(object)) { continue; } - const auto & detection_areas = areas_opt.value().detection_areas; - const auto & conflict_areas = areas_opt.value().conflict_areas; - const bool exist_in_detection_area = + // right direction + const bool exist_in_right_detection_area = std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { return bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), lanelet::utils::to2D(area)); }); - const bool exist_in_conflict_area = + // opposite direction + const bool exist_in_opposite_detection_area = std::any_of( + opposite_detection_areas.begin(), opposite_detection_areas.end(), + [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + const bool exist_in_detection_area = + exist_in_right_detection_area || exist_in_opposite_detection_area; + if (!exist_in_detection_area) { + continue; + } + const bool exist_in_right_conflict_area = isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_opposite_conflict_area = isPredictedPathInArea( + object, opposite_conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_conflict_area = + exist_in_right_conflict_area || exist_in_opposite_conflict_area; if (exist_in_detection_area && exist_in_conflict_area) { - obstacle_detected = true; debug_data_.conflicting_targets.objects.push_back(object); + return true; } } - return obstacle_detected; + return false; } else { return false; } @@ -504,6 +502,37 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( return new_lanelet; } +lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = + std::min(planner_param_.opposite_adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::RIGHT + ? lanelet.rightBound().invert() + : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width) + : lanelet.leftBound().invert(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : right_bound_) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + std::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, @@ -512,19 +541,20 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( { std::vector lane_ids; lanelet::ConstLanelets blind_spot_lanelets; + lanelet::ConstLanelets blind_spot_opposite_lanelets; /* get lane ids until intersection */ for (const auto & point : path.points) { bool found_intersection_lane = false; for (const auto lane_id : point.lane_ids) { - // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); - } - if (lane_id == lane_id_) { found_intersection_lane = true; + lane_ids.push_back(lane_id); break; } + // make lane_ids unique + if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { + lane_ids.push_back(lane_id); + } } if (found_intersection_lane) break; } @@ -537,27 +567,49 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( // additional detection area on left/right side lanelet::ConstLanelets adjacent_lanelets; + lanelet::ConstLanelets opposite_adjacent_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); - const auto adj = std::invoke([&]() -> std::optional { - if (turn_direction_ == TurnDirection::INVALID) { + const auto adj = + turn_direction_ == TurnDirection::LEFT + ? (routing_graph_ptr->adjacentLeft(lane)) + : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) + : boost::none); + const std::optional opposite_adj = + [&]() -> std::optional { + if (!!adj) { return std::nullopt; } - const auto adj_lane = (turn_direction_ == TurnDirection::LEFT) - ? routing_graph_ptr->adjacentLeft(lane) - : routing_graph_ptr->adjacentRight(lane); - - if (adj_lane) { - return *adj_lane; + if (turn_direction_ == TurnDirection::LEFT) { + // this should exist in right-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); } - - return std::nullopt; - }); - + if (turn_direction_ == TurnDirection::RIGHT) { + // this should exist in left-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } else { + return std::nullopt; + } + }(); if (adj) { const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); adjacent_lanelets.push_back(half_lanelet); } + if (opposite_adj) { + const auto half_lanelet = + generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_); + opposite_adjacent_lanelets.push_back(half_lanelet); + } } const auto current_arc_ego = @@ -587,6 +639,24 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj)); blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj)); } + // additional detection area on left/right opposite lane side + if (!opposite_adjacent_lanelets.empty()) { + const auto stop_line_arc_opposite_adj = + lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets); + const auto current_arc_opposite_adj = + stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego); + const auto detection_area_start_length_opposite_adj = + stop_line_arc_opposite_adj - planner_param_.backward_length; + auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj); + auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, detection_area_start_length_opposite_adj, + stop_line_arc_opposite_adj); + blind_spot_polygons.opposite_conflict_areas.emplace_back( + std::move(conflicting_area_opposite_adj)); + blind_spot_polygons.opposite_detection_areas.emplace_back( + std::move(detection_area_opposite_adj)); + } return blind_spot_polygons; } else { return std::nullopt; @@ -677,7 +747,7 @@ lanelet::ConstLanelets BlindSpotModule::getStraightLanelets( const auto next_lanelets = routing_graph_ptr->following(prev_intersection_lanelets.front()); for (const auto & ll : next_lanelets) { const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (!turn_direction.compare("straight")) { + if (turn_direction.compare("straight") == 0) { straight_lanelets.push_back(ll); } } diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 3915acd3532b5..6f8450568939c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -38,6 +39,8 @@ struct BlindSpotPolygons { std::vector conflict_areas; std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; }; class BlindSpotModule : public SceneModuleInterface @@ -50,8 +53,10 @@ class BlindSpotModule : public SceneModuleInterface geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; - std::vector conflict_areas_for_blind_spot; - std::vector detection_areas_for_blind_spot; + std::vector conflict_areas; + std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; @@ -67,6 +72,7 @@ class BlindSpotModule : public SceneModuleInterface double threshold_yaw_diff; //! threshold of yaw difference between ego and target object double adjacent_extend_width; //! the width of extended detection/conflict area on adjacent lane + double opposite_adjacent_extend_width; }; BlindSpotModule( @@ -118,6 +124,8 @@ class BlindSpotModule : public SceneModuleInterface lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. @@ -169,9 +177,9 @@ class BlindSpotModule : public SceneModuleInterface * @param pass_judge_line_idx generated pass judge line index * @return false when generation failed */ - bool generateStopLine( + std::optional> generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const; + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; /** * @brief Insert a point to target path diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 5f82fe839fda6..ce231659ccf78 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -1,233 +1,274 @@ -## Crosswalk +# Crosswalk -### Role +## Role -This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage of pedestrians and bicycles based on object's behavior and surround traffic. +This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage for crosswalk users, such as pedestrians and bicycles, based on the objects' behavior and surround traffic.
- ![example](docs/example.png){width=1000} -
crosswalk module
+ ![crosswalk_module](docs/crosswalk_module.svg){width=1100}
-### Activation Timing +## Features -The manager launch crosswalk scene modules when the reference path conflicts crosswalk lanelets. +### Yield -### Module Parameters +#### Target Object -#### Common parameters +The crosswalk module handles objects of the types defined by the following parameters in the `object_filtering.target_object` namespace. -| Parameter | Type | Description | -| ----------------------------- | ---- | ------------------------------- | -| `common.show_processing_time` | bool | whether to show processing time | +| Parameter | Unit | Type | Description | +| ------------ | ---- | ---- | ---------------------------------------------- | +| `unknown` | [-] | bool | whether to look and stop by UNKNOWN objects | +| `pedestrian` | [-] | bool | whether to look and stop by PEDESTRIAN objects | +| `bicycle` | [-] | bool | whether to look and stop by BICYCLE objects | +| `motorcycle` | [-] | bool | whether to look and stop by MOTORCYCLE objects | -#### Parameters for input data +In order to handle the crosswalk users crossing the neighborhood but outside the crosswalk, the crosswalk module creates an attention area around the crosswalk, shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield. -| Parameter | Type | Description | -| ------------------------------------ | ------ | ---------------------------------------------- | -| `common.traffic_light_state_timeout` | double | [s] timeout threshold for traffic light signal | +
+ ![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=600} +
-#### Parameters for stop position +The neighborhood is defined by the following parameter in the `object_filtering.target_object` namespace. -The crosswalk module determines a stop position at least `stop_distance_from_object` away from the object. +| Parameter | Unit | Type | Description | +| --------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------- | +| `crosswalk_attention_range` | [m] | double | the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk | -
- ![stop_distance_from_object](docs/stop_margin.svg){width=1000} -
stop margin
-
+#### Stop Position -The stop line is the reference point for the stopping position of the vehicle, but if there is no stop line in front of the crosswalk, the position `stop_distance_from_crosswalk` meters before the crosswalk is the virtual stop line for the vehicle. Then, if the stop position determined from `stop_distance_from_object` exists in front of the stop line determined from the HDMap or `stop_distance_from_crosswalk`, the actual stop position is determined according to `stop_distance_from_object` in principle, and vice versa. +First of all, `stop_distance_from_object [m]` is always kept at least between the ego and the target object for safety. -
- ![stop_line](docs/stop_line.svg){width=700} -
explicit stop line
-
+When the stop line exists in the lanelet map, the stop position is calculated based on the line. +When the stop line does **NOT** exist in the lanelet map, the stop position is calculated by keeping `stop_distance_from_crosswalk [m]` between the ego and the crosswalk. -
- ![stop_distance_from_crosswalk](docs/stop_line_distance.svg){width=700} -
virtual stop point
-
+
+ + + + + +
+
-On the other hand, if pedestrian (bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined to be `stop_distance_from_object` and pedestrian position, not at the stop line. +As an exceptional case, if a pedestrian (or bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined by `stop_distance_from_object` and pedestrian position, not at the stop line.
- ![far_object_threshold](docs/stop_line_margin.svg){width=1000} -
stop at wide crosswalk
+ ![far_object_threshold](docs/far_object_threshold.drawio.svg){width=700}
-See the workflow in algorithms section. +In the `stop_position` namespace, the following parameters are defined. -| Parameter | Type | Description | -| -------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_position.stop_distance_from_object` | double | [m] the vehicle decelerates to be able to stop in front of object with margin | -| `stop_position.stop_distance_from_crosswalk` | double | [m] make stop line away from crosswalk when no explicit stop line exists | -| `stop_position.far_object_threshold` | double | [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) | -| `stop_position.stop_position_threshold` | double | [m] threshold for check whether the vehicle stop in front of crosswalk | +| Parameter | | Type | Description | +| ------------------------------ | --- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_position_threshold` | [m] | double | If the ego vehicle has stopped near the stop line than this value, this module assumes itself to have achieved yielding. | +| `stop_distance_from_crosswalk` | [m] | double | make stop line away from crosswalk for the Lanelet2 map with no explicit stop lines | +| `far_object_threshold` | [m] | double | If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) for the case where the crosswalk width is very wide | +| `stop_distance_from_object` | [m] | double | the vehicle decelerates to be able to stop in front of object with margin | -#### Parameters for ego's slow down velocity +#### Yield decision -| Parameter | Type | Description | -| --------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- | -| `slow_velocity` | double | [m/s] target vehicle velocity when module receive slow down command from FOA | -| `max_slow_down_jerk` | double | [m/sss] minimum jerk deceleration for safe brake | -| `max_slow_down_accel` | double | [m/ss] minimum accel deceleration for safe brake | -| `no_relax_velocity` | double | [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | +The module makes a decision to yield only when the pedestrian traffic light is **GREEN** or **UNKNOWN**. +The decision is based on the following variables, along with the calculation of the collision point. -#### Parameters for stuck vehicle +- Time-To-Collision (TTC): The time for the **ego** to reach the virtual collision point. +- Time-To-Vehicle (TTV): The time for the **object** to reach the virtual collision point. -If there are low speed or stop vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle (see following figure), closing the distance to that vehicle could cause Ego to be stuck on the crosswalk. So, in this situation, this module plans to stop before the crosswalk and wait until the vehicles move away, even if there are no pedestrians or bicycles. +We classify ego behavior at crosswalks into three categories according to the relative relationship between TTC and TTV [1]. -
- ![stuck_vehicle_attention_range](docs/stuck_vehicle_attention_range.svg){width=1000} -
stuck vehicle attention range
-
+- A. **TTC >> TTV**: The object has enough time to cross before the ego. + - No stop planning. +- B. **TTC ≒ TTV**: There is a risk of collision. + - **Stop point is inserted in the ego's path**. +- C. **TTC << TTV**: Ego has enough time to cross before the object. + - No stop planning. -| Parameter | Type | Description | -| ------------------------------------------------ | ------ | ---------------------------------------------------------------------- | -| `stuck_vehicle.stuck_vehicle_velocity` | double | [m/s] maximum velocity threshold whether the vehicle is stuck | -| `stuck_vehicle.max_stuck_vehicle_lateral_offset` | double | [m] maximum lateral offset for stuck vehicle position should be looked | -| `stuck_vehicle.stuck_vehicle_attention_range` | double | [m] the detection area is defined as X meters behind the crosswalk | +
+ + + + + +
+
-#### Parameters for pass judge logic +The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. +In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}`. +In the same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. +In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}`. -Also see algorithm section. +In the `pass_judge` namespace, the following parameters are defined. -| Parameter | Type | Description | -| ------------------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------- | -| `pass_judge.ego_pass_first_margin` | double | [s] time margin for ego pass first situation | -| `pass_judge.ego_pass_later_margin` | double | [s] time margin for object pass first situation | -| `pass_judge.stop_object_velocity_threshold` | double | [m/s] velocity threshold for the module to judge whether the objects is stopped | -| `pass_judge.min_object_velocity` | double | [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV.) | -| `pass_judge.timeout_no_intention_to_walk` | double | [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. | -| `pass_judge.timeout_ego_stop_for_yield` | double | [s] the amount of time which ego should be stopping to query whether it yields or not. | +| Parameter | | Type | Description | +| ---------------------------------- | ----- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `ego_pass_first_margin_x` | [[s]] | double | time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_margin_y` | [[s]] | double | time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_additional_margin` | [s] | double | additional time margin for ego pass first situation to suppress chattering | +| `ego_pass_later_margin_x` | [[s]] | double | time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) | +| `ego_pass_later_margin_y` | [[s]] | double | time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) | +| `ego_pass_later_additional_margin` | [s] | double | additional time margin for object pass first situation to suppress chattering | -#### Parameters for object filtering +### Smooth Yield Decision -As a countermeasure against pedestrians attempting to cross outside the crosswalk area, this module watches not only the crosswalk zebra area but also in front of and behind space of the crosswalk, and if there are pedestrians or bicycles attempting to pass through the watch area, this module judges whether ego should pass or stop. +If the object is stopped near the crosswalk but has no intention of walking, a situation can arise in which the ego continues to yield the right-of-way to the object. +To prevent such a deadlock situation, the ego will cancel yielding depending on the situation. -
- ![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=1000} -
crosswalk attention range
-
+#### Cases without traffic lights -This module mainly looks the following objects as target objects. There are also optional flags that enables the pass/stop decision for `motorcycle` and `unknown` objects. - -- pedestrian -- bicycle - -| Parameter | Type | Description | -| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------- | -| `crosswalk_attention_range` | double | [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk | -| `target/unknown` | bool | whether to look and stop by UNKNOWN objects | -| `target/bicycle` | bool | whether to look and stop by BICYCLE objects | -| `target/motorcycle` | bool | whether to look and stop MOTORCYCLE objects | -| `target/pedestrian` | bool | whether to look and stop PEDESTRIAN objects | - -### Inner-workings / Algorithms - -#### Stop position - -The stop position is determined by the existence of the stop line defined by the HDMap, the positional relationship between the stop line and the pedestrians and bicycles, and each parameter. - -```plantuml -start -:calculate stop point from **stop_distance_from_object** (POINT-1); -if (There is the stop line in front of the crosswalk?) then (yes) - :calculate stop point from stop line (POINT-2.1); -else (no) - :calculate stop point from **stop_distance_from_crosswalk** (POINT-2.2); -endif -if (The distance ego to **POINT-1** is shorter than the distance ego to **POINT-2**) then (yes) - :ego stops at POINT-1; -else if (The distance ego to **POINT-1** is longer than the distance ego to **POINT-2** + **far_object_threshold**) then (yes) - :ego stops at POINT-1; -else (no) - :ego stops at POINT-2; -endif -end -``` +For the object stopped around the crosswalk but has no intention to walk (\*1), after the ego has keep stopping to yield for a specific time (\*2), the ego cancels the yield and starts driving. -#### Pass judge logic +\*1: +The time is calculated by the interpolation of distance between the object and crosswalk with `distance_map_for_no_intention_to_walk` and `timeout_map_for_no_intention_to_walk`. -At first, this module determines whether the pedestrians or bicycles are likely to cross the crosswalk based on the color of the pedestrian traffic light signal related to the crosswalk. Only when the pedestrian traffic signal is **RED**, this module judges that the objects will not cross the crosswalk and skip the pass judge logic. +In the `pass_judge` namespace, the following parameters are defined. -Secondly, this module makes a decision as to whether ego should stop in front of the crosswalk or pass through based on the relative relationship between TTC(Time-To-Collision) and TTV(Time-To-Vehicle). The TTC is the time it takes for ego to reach the virtual collision point, and the TTV is the time it takes for the object to reach the virtual collision point. +| Parameter | | Type | Description | +| --------------------------------------- | ----- | ------ | --------------------------------------------------------------------------------- | +| `distance_map_for_no_intention_to_walk` | [[m]] | double | distance map to calculate the timeout for no intention to walk with interpolation | +| `timeout_map_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | -
- ![virtual_collision_point](docs/virtual_collision_point.svg){width=1000} -
virtual collision point
-
+\*2: +In the `pass_judge` namespace, the following parameters are defined. -Depending on the relative relationship between TTC and TTV, the ego's behavior at crosswalks can be classified into three categories. +| Parameter | | Type | Description | +| ---------------------------- | --- | ------ | ----------------------------------------------------------------------------------------------------------------------- | +| `timeout_ego_stop_for_yield` | [s] | double | If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. | -1. **TTC >> TTV**: The objects have enough time to cross first before ego reaches the crosswalk. (Type-A) -2. **TTC ≒ TTV**: There is a risk of a near miss and collision between ego and objects at the virtual collision point. (Type-B) -3. **TTC << TTV**: Ego has enough time to path through the crosswalk before the objects reach the virtual collision point. (Type-C) +#### Cases with traffic lights -This module judges that ego is able to pass through the crosswalk without collision risk when the relative relationship between TTC and TTV is **Type-A** and **Type-C**. On the other hand, this module judges that ego needs to stop in front of the crosswalk prevent collision with objects in **Type-B** condition. The time margin can be set by parameters `ego_pass_first_margin` and `ego_pass_later_margin`. This logic is designed based on [1]. +The ego will cancel the yield without stopping when the object stops around the crosswalk but has no intention to walk (\*1). +This comes from the assumption that the object has no intention to walk since it is stopped even though the pedestrian traffic light is green. -
- ![ttc-ttv](docs/ttc-ttv.svg){width=1000} -
time-to-collision vs time-to-vehicle
-
+\*1: +The crosswalk user's intention to walk is calculated in the same way as `Cases without traffic lights`. -This module uses the larger value of estimated object velocity and `min_object_velocity` in calculating TTV in order to avoid division by zero. - -```plantuml -start -if (Pedestrian's traffic light signal is **RED**?) then (yes) -else (no) - if (There are objects around the crosswalk?) then (yes) - :calculate TTC & TTV; - if (TTC < TTV + **ego_pass_first_margin** && TTV < TTC + **ego_pass_later_margin**) then (yes) - :STOP; - else (no) - :PASS; - endif - endif -endif -end -``` +
+ + + + + +
+
+ +#### New Object Handling + +Due to the perception's limited performance where the tree or poll is recognized as a pedestrian or the tracking failure in the crowd or occlusion, even if the surrounding environment does not change, the new pedestrian (= the new ID's pedestrian) may suddenly appear unexpectedly. +If this happens while the ego is going to pass the crosswalk, the ego will stop suddenly. + +To deal with this issue, the option `disable_yield_for_new_stopped_object` is prepared. +If true is set, the yield decisions around the crosswalk with a traffic light will ignore the new stopped object. -#### Dead lock prevention +In the `pass_judge` namespace, the following parameters are defined. -If there are objects stop within a radius of `min_object_velocity * ego_pass_later_margin` meters from virtual collision point, this module judges that ego should stop based on the pass judge logic described above at all times. In such a situation, even if the pedestrian has no intention of crossing, ego continues the stop decision on the spot. So, this module has another logic for dead lock prevention, and if the object continues to stop for more than `timeout_no_intention_to_walk` seconds after ego stops in front of the crosswalk, this module judges that the object has no intention of crossing and switches from **STOP** state to **PASS** state. The parameter `stop_object_velocity_threshold` is used to judge whether the objects are stopped or not. In addition, if the object starts to move after the module judges that the object has no intention of crossing, this module judges whether ego should stop or not once again. +| Parameter | | Type | Description | +| -------------------------------------- | --- | ---- | ------------------------------------------------------------------------------------------------ | +| `disable_yield_for_new_stopped_object` | [-] | bool | If set to true, the new stopped object will be ignored around the crosswalk with a traffic light | + +### Safety Slow Down Behavior + +In the current autoware implementation, if no target object is detected around a crosswalk, the ego vehicle will not slow down for the crosswalk. +However, it may be desirable to slow down in situations, for example, where there are blind spots. +Such a situation can be handled by setting some tags to the related crosswalk as instructed in the [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) +document. + +| Parameter | | Type | Description | +| --------------------- | ------- | ------ | --------------------------------------------------------------------------------------------------------------------- | +| `slow_velocity` | [m/s] | double | target vehicle velocity when module receive slow down command from FOA | +| `max_slow_down_jerk` | [m/sss] | double | minimum jerk deceleration for safe brake | +| `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | +| `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | + +### Stuck Vehicle Detection + +The feature will make the ego not to stop on the crosswalk. +When there is a low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module plans to stop before the crosswalk even if there are no pedestrians or bicycles. + +`min_acc`, `min_jerk`, and `max_jerk` are met. If the ego cannot stop before the crosswalk with these parameters, the stop position will move forward.
- ![no-intension](docs/no-intension.svg){width=1000} -
dead lock situation
+ ![stuck_vehicle_attention_range](docs/stuck_vehicle_detection.svg){width=600}
-#### Safety Slow Down Behavior +In the `stuck_vehicle` namespace, the following parameters are defined. -In current autoware implementation if there are no target objects around a crosswalk, ego vehicle -will not slow down for the crosswalk. However, if ego vehicle to slow down to a certain speed in -such cases is wanted then it is possible by adding some tags to the related crosswalk definition as -it is instructed -in [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) -document. +| Parameter | Unit | Type | Description | +| ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | +| `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 | +| `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 | + +### Others + +In the `common` namespace, the following parameters are defined. + +| Parameter | Unit | Type | Description | +| ----------------------------- | ---- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `show_processing_time` | [-] | bool | whether to show processing time | +| `traffic_light_state_timeout` | [s] | double | timeout threshold for traffic light signal | +| `enable_rtc` | [-] | bool | if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. | -### Limitations +## Known Issues -When multiple crosswalks are nearby (such as intersection), this module may make a stop decision even at crosswalks where the object has no intention of crossing. +- The yield decision may be sometimes aggressive or conservative depending on the case. + - The main reason is that the crosswalk module does not know the ego's position in the future. The detailed ego's position will be determined after the whole planning. + - Currently the module assumes that the ego will move with a constant velocity. + +## Debugging + +### Visualization of debug markers + +`/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk` shows the following markers.
- ![limitation](docs/limitation.svg){width=1000} -
design limits
+ ![limitation](docs/debug_markers.png){width=1000}
-### Known Issues +- Yellow polygons + - Ego footprints' polygon to calculate the collision check. +- Pink polygons + - Object footprints' polygon to calculate the collision check. +- The color of crosswalks + - Considering the traffic light's color, red means the target crosswalk, and white means the ignored crosswalk. +- Texts + - It shows the module ID, TTC, TTV, and the module state. -### Debugging +### Visualization of Time-To-Collision + +```sh +ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py +``` -By `ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py`, you can visualize the following figure of the ego and pedestrian's time to collision. +enables you to visualize the following figure of the ego and pedestrian's time to collision. The label of each plot is `-`.
![limitation](docs/time_to_collision_plot.png){width=1000} -
Plot of time to collision
-### References/External links +## Trouble Shooting + +### Behavior + +- Q. The ego stopped around the crosswalk even though there were no crosswalk user objects. + - A. See [Stuck Vehicle Detection](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection). +- Q. The crosswalk virtual wall suddenly appeared resulting in the sudden stop. + - A. There may be a crosswalk user started moving when the ego was close to the crosswalk. +- Q. The crosswalk module decides to stop even when the pedestrian traffic light is red. + - A. The lanelet map may be incorrect. The pedestrian traffic light and the crosswalk have to be related. +- Q. In the planning simulation, the crosswalk module does the yield decision to stop on all the crosswalks. + - A. This is because the pedestrian traffic light is unknown by default. In this case, the crosswalk does the yield decision for safety. + +### Parameter Tuning + +- Q. The ego's yield behavior is too conservative. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection) +- Q. The ego's yield behavior is too aggressive. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection) + +## References/External links [1] 佐藤 みなみ, 早坂 祥一, 清水 政行, 村野 隆彦, 横断歩行者に対するドライバのリスク回避行動のモデル化, 自動車技術会論文集, 2013, 44 巻, 3 号, p. 931-936. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 240963309e9a2..aa5dfed1e4bc5 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -16,7 +16,7 @@ # For the case where the crosswalk width is very wide far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. - stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: @@ -38,7 +38,7 @@ # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles pass_judge: ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [3.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) @@ -50,19 +50,19 @@ min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] - stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) + stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding disable_stop_for_yield_cancel: true # for the crosswalks with traffic signal disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order - timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] - timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. + timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] + timeout_ego_stop_for_yield: 1.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. # param for target object filtering object_filtering: - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + crosswalk_attention_range: 2.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle target_object: unknown: true # [-] whether to look and stop by UNKNOWN objects diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg index 270a61264fe66..26e6118756bd8 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg @@ -1,213 +1,132 @@ + - + - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -
+
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%22%20style%3D%22endArrow%3Dclassic%3Bhtml%3D1%3Brounded%3D0%3BexitX%3D0.25%3BexitY%3D0%3BexitDx%3D0%3BexitDy%3D0%3BentryX%3D0.5%3BentryY%3D0.442%3BentryDx%3D0%3BentryDy%3D0%3BentryPerimeter%3D0%3B%22%20edge%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20width%3D%2250%22%20height%3D%2250%22%20relative%3D%221%22%20as%3D%22geometry%22%3E%3CmxPoint%20x%3D%22851%22%20y%3D%221780%22%20as%3D%22sourcePoint%22%2F%3E%3CmxPoint%20x%3D%22890.04%22%20y%3D%221780%22%20as%3D%22targetPoint%22%2F%3E%3C%2FmxGeometry%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - +
+ target
- %3CmxGraphModel%3E%3... + target - - - - - - -
-
- crosswalk_attention_range - [m] +
+ target
- crosswalk_attention_range [m] + target - - - - -
+
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - +
+ NOT + target
- %3Cmx... + NOT target - - -
+
-
- Module don't respond prediction path without attention range. -
-
-
- - Module don't respond predict... - - - - - - -
-
-
- Vehicle +
+ crosswalk_attention_range
- Vehicle + crosswalk_attention... - - + + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg new file mode 100644 index 0000000000000..18225f188fddf --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png b/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png new file mode 100644 index 0000000000000..15645a340dca8 Binary files /dev/null and b/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png differ diff --git a/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg new file mode 100644 index 0000000000000..13ba18bbb7588 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line defined in the lanelet map +
+
+
+ + stop line defined in the lanele... + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg b/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg new file mode 100644 index 0000000000000..6e4c152cd79f0 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line defined in the lanelet map +
+
+
+ + stop line defined in the lanele... + + + + + + + + + + + + + +
+
+
+ stop_distance_from_object +
+
+
+ + stop_distance_from_... + + + + + + + + + + + + + +
+
+
far_object_threshold <
+
+
+ + far_object_threshold < + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/limitation.svg b/planning/behavior_velocity_crosswalk_module/docs/limitation.svg deleted file mode 100644 index 3e507c0669a2b..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/limitation.svg +++ /dev/null @@ -1,164 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - -
-
-
- stop objects -
-
-
- - stop objects - - - - - - - - - - - -
-
-
- waiting for the traffic light to turn green. -
-
-
- - waiting for the traffic l... - - - - - -
-
-
- The pedestrian has no intention to cross via this route. -
-
-
- - The pedestrian has no intent... - - - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg b/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg deleted file mode 100644 index 803ce223d591a..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg +++ /dev/null @@ -1,193 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - - - - - - - -
-
-
- min_object_velocity * ego_pass_later_margin - [m] -
-
-
- - min_object_velocity * ego_pass_later_margin [... - - - - - - -
-
-
- yield... -
-
-
- - yield... - - - - - -
-
-
- stop objects -
-
-
- - stop objects - - - - - - - -
-
-
- don't have intention -
- to cross -
-
-
- - don't have intentio... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg deleted file mode 100644 index c2d486a7d3541..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg +++ /dev/null @@ -1,149 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - - - - - -
-
-
- stop line -
- (defined by HDMap) -
-
-
- - stop line... - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg deleted file mode 100644 index 3842601ae246c..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg +++ /dev/null @@ -1,170 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - - - - - - -
-
-
- stop_distance_from_crosswalk - [m] -
-
-
- - stop_distance_from_crosswalk [m] - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - -
-
-
- stop point -
- (planned in module) -
-
-
- - stop point... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg deleted file mode 100644 index ea7fd9be3ff41..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg +++ /dev/null @@ -1,204 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - -
-
-
- far_object_threshold - [m] -
-
-
- - far_object_threshold [m] - - - - - - - - - - -
-
-
- stop line -
- (defined by HDMap) -
-
-
- - stop line... - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg deleted file mode 100644 index 924f642ad08bb..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg +++ /dev/null @@ -1,144 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
- - stop_distance_from_object [m] - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - -
-
-
- The module determines a stop position at least - stop_distance_from_object - away from the object's predicted path. -
-
-
- - The module determines a stop positio... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg deleted file mode 100644 index 3ac1a94fcec78..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg +++ /dev/null @@ -1,174 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%22%20style%3D%22endArrow%3Dclassic%3Bhtml%3D1%3Brounded%3D0%3BexitX%3D0.25%3BexitY%3D0%3BexitDx%3D0%3BexitDy%3D0%3BentryX%3D0.5%3BentryY%3D0.442%3BentryDx%3D0%3BentryDy%3D0%3BentryPerimeter%3D0%3B%22%20edge%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20width%3D%2250%22%20height%3D%2250%22%20relative%3D%221%22%20as%3D%22geometry%22%3E%3CmxPoint%20x%3D%22851%22%20y%3D%221780%22%20as%3D%22sourcePoint%22%2F%3E%3CmxPoint%20x%3D%22890.04%22%20y%3D%221780%22%20as%3D%22targetPoint%22%2F%3E%3C%2FmxGeometry%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20i... - - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - - -
-
-
- stuck_vehicle_attention_range - [m] -
-
-
- - stuck_vehicle_attention_range [... - - - - - - -
-
-
- Vehicle -
-
-
- - Vehicle - - - - - - - - - -
-
-
- max_stuck_vehicle_lateral_offset - [m] -
-
-
- - max_stuck_vehicle_lateral_offset [m] - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg new file mode 100644 index 0000000000000..c517be5bb9967 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stuck_vehicle_attention_range +
+
+
+ + stuck_vehicle_atten... + + + + + + + + + + + + + + +
+
+
+ max_stuck_vehicle_lateral_offset +
+
+
+ + max_stuck_vehicle_l... + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg b/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg new file mode 100644 index 0000000000000..6eb1b25cf5642 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg @@ -0,0 +1,210 @@ + + + + + + + + + + + + +
+
+
+ (5, 1) +
+
+
+ + (5, 1) + + + + + + +
+
+
+ (1, 4) +
+
+
+ + (1, 4) + + + + + + +
+
+
+ (2, 6) +
+
+
+ + (2, 6) + + + + + + + + + + +
+
+
+ (0, 1) +
+
+
+ + (0, 1) + + + + + + +
+
+
+ (3, 0) +
+
+
+ + (3, 0) + + + + + + +
+
+
+ Time-To-Vehicle [s] +
+
+
+ + Time-To-Vehicle [s] + + + + + + + + + + + + + + + + + + + +
+
+
+ Time-To-Collision [s] +
+
+
+ + Time-To-Collision [s] + + + + + + +
+
+
+ B +
+
+
+ + B + + + + + + +
+
+
+ A +
+
+
+ + A + + + + + + +
+
+
+ C +
+
+
+ + C + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg b/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg index 8084dc2fbd8b0..c409493483845 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg @@ -1,134 +1,65 @@ + - + - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
- - %3Cmx... - - - - - - - - - - - -
-
-
- virtual collision point -
-
-
- - virtual collision poi... - - - - - -
-
-
- path -
-
-
- - path - - - - - -
+
-
- object's prediction path +
+ virtual collision point
- object's prediction p... + virtual collision point - + diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg new file mode 100644 index 0000000000000..7d4b4017c2f1a --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop_distance_from_crosswalk +
+
+
+ + stop_distance_from_... + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg b/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg new file mode 100644 index 0000000000000..a09f430e990b3 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ NOT + yield +
+
+
+ + NOT yield + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg b/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg new file mode 100644 index 0000000000000..d4dea5fb78d2e --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ yield +
+
+
+ + yield + + + + + + + + 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 b8190cb6124e7..dc3ce32be8505 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".common.enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7175cd0895fc0..48ae5ff03b8bd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -766,7 +766,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint( const std::optional object_crosswalk_passage_direction) const { constexpr double min_ego_velocity = 1.38; // [m/s] - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity); @@ -774,8 +773,12 @@ CollisionPoint CrosswalkModule::createCollisionPoint( CollisionPoint collision_point{}; collision_point.collision_point = nearest_collision_point; collision_point.crosswalk_passage_direction = object_crosswalk_passage_direction; + + // The decision of whether the ego vehicle or the pedestrian goes first is determined by the logic + // for ego_pass_first or yield; even the decision for ego_pass_later does not affect this sense. + // Hence, here, we use the length that would be appropriate for the ego_pass_first judge. collision_point.time_to_collision = - std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) / + std::max(0.0, dist_ego2cp - planner_data_->vehicle_info_.min_longitudinal_offset_m) / std::max(ego_vel.x, min_ego_velocity); collision_point.time_to_vehicle = std::max(0.0, dist_obj2cp) / velocity; @@ -810,7 +813,9 @@ void CrosswalkModule::applySafetySlowDownSpeed( const auto & p_safety_slow = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, safety_slow_point_range); - insertDecelPointWithDebugInfo(p_safety_slow.value(), safety_slow_down_speed, output); + if (p_safety_slow.has_value()) { + insertDecelPointWithDebugInfo(p_safety_slow.value(), safety_slow_down_speed, output); + } if (safety_slow_point_range < 0.0) { passed_safety_slow_point_ = true; diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index e7ec6b37f7f20..834ffc03e5dde 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 9e7eb196cd0c1..4c8fe5c6fa0f5 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -8,11 +8,16 @@ pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) find_package(OpenCV REQUIRED) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp src/manager.cpp + src/util.cpp src/scene_intersection.cpp + src/intersection_lanelets.cpp + src/scene_intersection_prepare_data.cpp + src/scene_intersection_stuck.cpp + src/scene_intersection_occlusion.cpp + src/scene_intersection_collision.cpp src/scene_merge_from_private_road.cpp - src/util.cpp + src/debug.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 58c2ce59edd48..afb2381e628a4 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -88,7 +88,7 @@ To precisely calculate stop positions, the path is interpolated at the certain i - closest_idx denotes the path point index which is closest to ego position. - first_attention_stopline denotes the first path point where ego footprint intersects with the attention_area. -- If a stopline is associated with the intersection lane on the map, that line is used as the default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as the default_stopline instead. +- If a stopline is associated with the intersection lane on the map, that line is used as default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as default_stopline instead. - occlusion_peeking_stopline is a bit ahead of first_attention_stopline as described later. - occlusion_wo_tl_pass_judge_line is the first position where ego footprint intersects with the centerline of the first attention_area lane. @@ -113,8 +113,8 @@ There are several behaviors depending on the scene. | Safe | Ego detected no occlusion and collision | Ego passes the intersection | | StuckStop | The exit of the intersection is blocked by traffic jam | Ego stops before the intersection or the boundary of attention area | | YieldStuck | Another vehicle stops to yield ego | Ego stops before the intersection or the boundary of attention area | -| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at the default_stop_line | -| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at the default_stop_line at first | +| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at default_stopline | +| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at default_stopline at first | | PeekingTowardOcclusion | Ego detected occlusion and but no collision within the FOV (after FirstWaitBeforeOcclusion) | Ego approaches the boundary of the attention area slowly | | OccludedCollisionStop | Ego detected both occlusion and collision (after FirstWaitBeforeOcclusion) | Ego stops immediately | | FullyPrioritized | Ego is fully prioritized by the RED/Arrow signal | Ego only cares vehicles still running inside the intersection. Occlusion is ignored | @@ -219,7 +219,7 @@ ros2 run behavior_velocity_intersection_module ttc.py --lane_id ## Occlusion detection -If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough ego first makes a brief stop at the default stop line for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stop_line. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stop_line. Otherwise only stop line is inserted. +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough ego first makes a brief stop at default_stopline for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stopline. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stopline. Otherwise only stop line is inserted. During the creeping if collision is detected this module inserts a stop line in front of ego immediately, and if the FOV gets sufficiently clear the intersection_occlusion wall will disappear. If occlusion is cleared and no collision is detected ego will pass the intersection. @@ -241,7 +241,7 @@ The remaining time is visualized on the intersection_occlusion virtual wall. ### Occlusion handling at intersection without traffic light -At intersection without traffic light, if occlusion is detected, ego makes a brief stop at the default_stopline and first_attention_stopline respectively. After stopping at the first_attention_area_stopline this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and occlusion_wo_tl_pass_judge_line while occlusion is not cleared. If collision is detected, ego immediately stops. Once the occlusion is cleared or ego has passed occlusion_wo_tl_pass_judge_line this module does not detect collision and occlusion because ego footprint is already inside the intersection. +At intersection without traffic light, if occlusion is detected, ego makes a brief stop at default_stopline and first_attention_stopline respectively. After stopping at the first_attention_area_stopline this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and occlusion_wo_tl_pass_judge_line while occlusion is not cleared. If collision is detected, ego immediately stops. Once the occlusion is cleared or ego has passed occlusion_wo_tl_pass_judge_line this module does not detect collision and occlusion because ego footprint is already inside the intersection. ![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) @@ -263,7 +263,7 @@ TTC parameter varies depending on the traffic light color/shape as follows. ### yield on GREEN -If the traffic light color changed to GREEN and ego approached the entry of the intersection lane within the distance `collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start` and there is any object whose distance to its stopline is less than `collision_detection.yield_on_green_traffic_light.object_dist_to_stopline`, this module commands to stop for the duration of `collision_detection.yield_on_green_traffic_light.duration` at the default_stopline. +If the traffic light color changed to GREEN and ego approached the entry of the intersection lane within the distance `collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start` and there is any object whose distance to its stopline is less than `collision_detection.yield_on_green_traffic_light.object_dist_to_stopline`, this module commands to stop for the duration of `collision_detection.yield_on_green_traffic_light.duration` at default_stopline. ### skip on AMBER @@ -281,18 +281,49 @@ When the traffic light color/shape is RED/Arrow, occlusion detection is skipped. ## Pass Judge Line -To avoid sudden braking, if deceleration and jerk more than the threshold (`common.max_accel` and `common.max_jerk`) is required to stop at first_attention_stopline, this module does not command to stop once it passed the default_stopline position. +Generally it is not tolerable for vehicles that have lower traffic priority to stop in the middle of the unprotected area in intersections, and they need to stop at the stop line beforehand if there will be any risk of collision, which introduces two requirements: -If ego passed pass_judge_line, then ego does not stop anymore. If ego passed pass_judge_line while ego is stopping for dangerous decision, then ego stops while the situation is judged as dangerous. Once the judgement turned safe, ego restarts and does not stop anymore. +1. The vehicle must start braking before the boundary of the unprotected area at least by the braking distance if it is supposed to stop +2. The vehicle must recognize upcoming vehicles and check safety beforehand with enough braking distance margin if it is supposed to go + 1. And the SAFE decision must be absolutely certain and remain to be valid for the future horizon so that the safety condition will be always satisfied while ego is driving inside the unprotected area. +3. (TODO): Since it is almost impossible to make perfectly safe decision beforehand given the limited detection range/velocity tracking performance, intersection module should plan risk-evasive acceleration velocity profile AND/OR relax lateral acceleration limit while ego is driving inside the unprotected area, if the safety decision is "betrayed" later due to the following reasons: + 1. The situation _turned out to be dangerous_ later, mainly because velocity tracking was underestimated or the object accelerated beyond TTC margin + 2. The situation _turned dangerous_ later, mainly because the object is suddenly detected out of nowhere -The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane. +The position which is before the boundary of unprotected area by the braking distance which is obtained by -- If `occlusion.enable` is false, the pass judge line before the `first_attention_stopline` by the braking distance $v_{ego}^{2} / 2a_{max}$. -- If `occlusion.enable` is true and: - - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stopline` in order to continue peeking/collision detection while occlusion is detected. - - if there are no associated traffic lights and: - - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking. - - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false. +$$ +\dfrac{v_{\mathrm{ego}}^{2}}{2a_{\mathrm{max}}} + v_{\mathrm{ego}} * t_{\mathrm{delay}} +$$ + +is called pass_judge_line, and safety decision must be made before ego passes this position because ego does not stop anymore. + +1st_pass_judge_line is before the first upcoming lane, and at intersections with multiple upcoming lanes, 2nd_pass_judge_line is defined as the position which is before the centerline of the first attention lane by the braking distance. 1st/2nd_pass_judge_line are illustrated in the following figure. + +![pass-judge-line](./docs/pass-judge-line.drawio.svg) + +Intersection module will command to GO if + +- ego is over default_stopline(or `common.enable_pass_judge_before_default_stopline` is true) AND +- ego is over 1st_pass judge line AND +- ego judged SAFE previously AND +- (ego is over 2nd_pass_judge_line OR ego is between 1st and 2nd pass_judge_line but most probable collision is expected to happen in the 1st attention lane) + +because it is expected to stop or continue stop decision if + +1. ego is before default_stopline && `common.enable_pass_judge_before_default_stopline` is false OR + 1. reason: default_stopline is defined on the map and should be respected +2. ego is before 1st_pass_judge_line OR + 1. reason: it has enough braking distance margin +3. ego judged UNSAFE previously + 1. reason: ego is now trying to stop and should continue stop decision if collision is detected in later calculation +4. (ego is between 1st and 2nd pass_judge_line and the most probable collision is expected to happen in the 2nd attention lane) + +For the 3rd condition, it is possible that ego stops with some overshoot to the unprotected area while it is trying to stop for collision detection, because ego should keep stop decision while UNSAFE decision is made even if it passed 1st_pass_judge_line during deceleration. + +For the 4th condition, at intersections with 2nd attention lane, even if ego is over the 1st pass_judge_line, still intersection module commands to stop if the most probable collision is expected to happen in the 2nd attention lane. + +Also if `occlusion.enable` is true, the position of 1st_pass_judge line changes to occlusion_peeking_stopline if ego passed the original 1st_pass_judge_line position while ego is peeking. Otherwise ego could inadvertently judge that it passed 1st_pass_judge during peeking and then abort peeking. ## Data Structure @@ -375,17 +406,18 @@ entity TargetObject { ### common -| Parameter | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------------ | -| `.attention_area_length` | double | [m] range for object detection | -| `.attention_area_margin` | double | [m] margin for expanding attention area width | -| `.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | -| `.use_intersection_area` | bool | [-] flag to use intersection_area for collision detection | -| `.default_stopline_margin` | double | [m] margin before_stop_line | -| `.stopline_overshoot_margin` | double | [m] margin for the overshoot from stopline | -| `.max_accel` | double | [m/ss] max acceleration for stop | -| `.max_jerk` | double | [m/sss] max jerk for stop | -| `.delay_response_time` | double | [s] action delay before stop | +| Parameter | Type | Description | +| -------------------------------------------- | ------ | -------------------------------------------------------------------------------- | +| `.attention_area_length` | double | [m] range for object detection | +| `.attention_area_margin` | double | [m] margin for expanding attention area width | +| `.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | +| `.use_intersection_area` | bool | [-] flag to use intersection_area for collision detection | +| `.default_stopline_margin` | double | [m] margin before_stop_line | +| `.stopline_overshoot_margin` | double | [m] margin for the overshoot from stopline | +| `.max_accel` | double | [m/ss] max acceleration for stop | +| `.max_jerk` | double | [m/sss] max jerk for stop | +| `.delay_response_time` | double | [s] action delay before stop | +| `.enable_pass_judge_before_default_stopline` | bool | [-] flag not to stop before default_stopline even if ego is over pass_judge_line | ### stuck_vehicle/yield_stuck @@ -430,7 +462,7 @@ entity TargetObject { | `.possible_object_bbox` | [double] | [m] minimum bounding box size for checking if occlusion polygon is small enough | | `.ignore_parked_vehicle_speed_threshold` | double | [m/s] velocity threshold for checking parked vehicle | | `.occlusion_detection_hold_time` | double | [s] hold time of occlusion detection | -| `.temporal_stop_time_before_peeking` | double | [s] temporal stop duration at the default_stop_line before starting peeking | +| `.temporal_stop_time_before_peeking` | double | [s] temporal stop duration at default_stopline before starting peeking | | `.temporal_stop_before_attention_area` | bool | [-] flag to temporarily stop at first_attention_stopline before peeking into attention_area | | `.creep_velocity_without_traffic_light` | double | [m/s] creep velocity to occlusion_wo_tl_pass_judge_line | | `.static_occlusion_with_traffic_light_timeout` | double | [s] the timeout duration for ignoring static occlusion at intersection with traffic light | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 4e9eb50f2a462..108e021240851 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,7 @@ max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 + enable_pass_judge_before_default_stopline: false stuck_vehicle: turn_direction: @@ -36,7 +37,6 @@ consider_wrong_direction_vehicle: false collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 - keep_detection_velocity_threshold: 0.833 velocity_profile: use_upstream: true minimum_upstream_velocity: 0.01 diff --git a/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg new file mode 100644 index 0000000000000..d1f488af7c2ac --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg @@ -0,0 +1,473 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+ + 1st_pass_judge_line + + + + + + + + + + +
+
+
+ + 2nd_pass_judge_line_magin + +
+
+
+ + 2nd_pass_judge_line_magin + + + + + + + + + + +
+
+
+ + 2nd_pass_judge_line + +
+
+
+ + 2nd_pass_judge_line + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+ + 1st_pass_judge_line + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 5f103e0ecd3b0..350f6d774f7cf 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -90,7 +90,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; marker_line.action = visualization_msgs::msg::Marker::ADD; marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); + marker_line.scale = createMarkerScale(0.2, 0.0, 0.0); marker_line.color = createMarkerColor(r, g, b, 0.999); const double yaw = tf2::getYaw(pose.orientation); @@ -283,11 +283,23 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); */ - if (debug_data_.pass_judge_wall_pose) { + 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( - debug_data_.pass_judge_wall_pose.value(), "pass_judge_wall_pose", module_id_, 0.7, 0.85, - 0.9), + debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, + g, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_pass_judge_wall_pose) { + 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( + debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, + r, g, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp new file mode 100644 index 0000000000000..48b0ecf1349a5 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -0,0 +1,203 @@ +// 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 DECISION_RESULT_HPP_ +#define DECISION_RESULT_HPP_ + +#include +#include +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief Internal error or ego already passed pass_judge_line + */ +struct Indecisive +{ + std::string error; +}; + +/** + * @struct + * @brief detected stuck vehicle + */ +struct StuckStop +{ + size_t closest_idx{0}; + size_t stuck_stopline_idx{0}; + std::optional occlusion_stopline_idx{std::nullopt}; +}; + +/** + * @struct + * @brief yielded by vehicle on the attention area + */ +struct YieldStuckStop +{ + size_t closest_idx{0}; + size_t stuck_stopline_idx{0}; +}; + +/** + * @struct + * @brief only collision is detected + */ +struct NonOccludedCollisionStop +{ + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief occlusion is detected so ego needs to stop at the default stop line position + */ +struct FirstWaitBeforeOcclusion +{ + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief ego is approaching the boundary of attention area in the presence of traffic light + */ +struct PeekingTowardOcclusion +{ + //! if intersection_occlusion is disapproved externally through RTC, it indicates + //! "is_forcefully_occluded" + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; + //! 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 and shows up + //! intersection_occlusion(x.y) + std::optional static_occlusion_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief both collision and occlusion are detected in the presence of traffic light + */ +struct OccludedCollisionStop +{ + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; + //! 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}; +}; + +/** + * @struct + * @brief at least occlusion is detected in the absence of traffic light + */ +struct OccludedAbsenceTrafficLight +{ + bool is_actually_occlusion_cleared{false}; + bool collision_detected{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t first_attention_area_stopline_idx{0}; + size_t peeking_limit_line_idx{0}; +}; + +/** + * @struct + * @brief both collision and occlusion are not detected + */ +struct Safe +{ + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief traffic light is red or arrow signal + */ +struct FullyPrioritized +{ + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +using DecisionResult = std::variant< + Indecisive, //! internal process error, or over the pass judge line + StuckStop, //! detected stuck vehicle + YieldStuckStop, //! detected yield stuck vehicle + NonOccludedCollisionStop, //! detected collision while FOV is clear + FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion + PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected + OccludedCollisionStop, //! occlusion and collision are both detected + OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light + Safe, //! judge as safe + 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 ""; +} + +} // namespace behavior_velocity_planner::intersection + +#endif // DECISION_RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp similarity index 71% rename from planning/behavior_velocity_intersection_module/src/util_type.hpp rename to planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index 763d829dacf9b..c47f016fbdbda 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -12,26 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTIL_TYPE_HPP_ -#define UTIL_TYPE_HPP_ +#ifndef INTERPOLATED_PATH_INFO_HPP_ +#define INTERPOLATED_PATH_INFO_HPP_ -#include - -#include #include -#include -#include -#include -#include -#include +#include #include #include #include -#include -namespace behavior_velocity_planner::util +namespace behavior_velocity_planner::intersection { /** @@ -52,6 +44,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner::util +} // namespace behavior_velocity_planner::intersection -#endif // UTIL_TYPE_HPP_ +#endif // INTERPOLATED_PATH_INFO_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp new file mode 100644 index 0000000000000..555ea424dfef0 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -0,0 +1,82 @@ +// 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 "intersection_lanelets.hpp" + +#include "util.hpp" + +#include +#include + +#include + +namespace behavior_velocity_planner::intersection +{ + +void IntersectionLanelets::update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + is_prioritized_ = is_prioritized; + // find the first conflicting/detection area polygon intersecting the path + if (!first_conflicting_area_) { + auto first = util::getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); + } + } + if (!first_attention_area_) { + const auto first = util::getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); + } + } + if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { + const auto first_attention_lane = first_attention_lane_.value(); + // remove first_attention_area_ and non-straight lanelets from attention_non_preceding + lanelet::ConstLanelets attention_non_preceding_ex_first; + lanelet::ConstLanelets sibling_first_attention_lanelets; + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & following : routing_graph_ptr->following(previous)) { + sibling_first_attention_lanelets.push_back(following); + } + } + for (const auto & ll : attention_non_preceding_) { + // the sibling lanelets of first_attention_lanelet are ruled out + if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { + continue; + } + if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { + attention_non_preceding_ex_first.push_back(ll); + } + } + if (attention_non_preceding_ex_first.empty()) { + second_attention_lane_empty_ = true; + } + const auto attention_non_preceding_ex_first_area = + util::getPolygon3dFromLanelets(attention_non_preceding_ex_first); + const auto second = util::getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); + if (second) { + second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); + second_attention_area_ = second_attention_lane_.value().polygon3d(); + } + } +} +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp new file mode 100644 index 0000000000000..11deae6bdc001 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -0,0 +1,197 @@ +// 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 INTERSECTION_LANELETS_HPP_ +#define INTERSECTION_LANELETS_HPP_ + +#include "interpolated_path_info.hpp" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief see the document for more details of IntersectionLanelets + */ +struct IntersectionLanelets +{ +public: + /** + * update conflicting lanelets and traffic priority information + */ + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr); + + const lanelet::ConstLanelets & attention() const + { + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stoplines() const + { + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; + } + const lanelet::ConstLanelets & conflicting() const { return conflicting_; } + const lanelet::ConstLanelets & adjacent() const { return adjacent_; } + const lanelet::ConstLanelets & occlusion_attention() const + { + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; + } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } + const std::vector & attention_area() const + { + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; + } + const std::vector & conflicting_area() const + { + return conflicting_area_; + } + const std::vector & adjacent_area() const { return adjacent_area_; } + const std::vector & occlusion_attention_area() const + { + return occlusion_attention_area_; + } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } + const std::optional & first_conflicting_area() const + { + return first_conflicting_area_; + } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } + const std::optional & second_attention_lane() const + { + return second_attention_lane_; + } + const std::optional & second_attention_area() const + { + return second_attention_area_; + } + + /** + * the set of attention lanelets which is topologically merged + */ + lanelet::ConstLanelets attention_; + std::vector attention_area_; + + /** + * the stop lines for each attention_lanelets associated with traffic lights. At intersection + * without traffic lights, each value is null + */ + std::vector> attention_stoplines_; + + /** + * the conflicting part of attention lanelets + */ + lanelet::ConstLanelets attention_non_preceding_; + std::vector attention_non_preceding_area_; + + /** + * the stop lines for each attention_non_preceding_ + */ + std::vector> attention_non_preceding_stoplines_; + + /** + * the conflicting lanelets of the objective intersection lanelet + */ + lanelet::ConstLanelets conflicting_; + std::vector conflicting_area_; + + /** + * + */ + lanelet::ConstLanelets adjacent_; + std::vector adjacent_area_; + + /** + * the set of attention lanelets for occlusion detection which is topologically merged + */ + lanelet::ConstLanelets occlusion_attention_; + std::vector occlusion_attention_area_; + + /** + * the vector of sum of each occlusion_attention lanelet + */ + std::vector occlusion_attention_size_; + + /** + * the first conflicting lanelet which ego path points intersect for the first time + */ + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + + /** + * the first attention lanelet which ego path points intersect for the first time + */ + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + + /** + * the second attention lanelet which ego path points intersect next to the + * first_attention_lanelet + */ + bool second_attention_lane_empty_{false}; + std::optional second_attention_lane_{std::nullopt}; + std::optional second_attention_area_{std::nullopt}; + + /** + * flag if the intersection is prioritized by the traffic light + */ + bool is_prioritized_{false}; +}; + +/** + * @struct + * @brief see the document for more details of PathLanelets + */ +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the + // path +}; +} // namespace behavior_velocity_planner::intersection + +#endif // INTERSECTION_LANELETS_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp new file mode 100644 index 0000000000000..64d20b81e3fad --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -0,0 +1,78 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef INTERSECTION_STOPLINES_HPP_ +#define INTERSECTION_STOPLINES_HPP_ + +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct IntersectionStopLines +{ + size_t closest_idx{0}; + + /** + * stuck_stopline is null if ego path does not intersect with first_conflicting_area + */ + std::optional stuck_stopline{std::nullopt}; + + /** + * default_stopline is null if it is calculated negative from first_attention_stopline + */ + std::optional default_stopline{std::nullopt}; + + /** + * first_attention_stopline is null if ego footprint along the path does not intersect with + * attention area. if path[0] satisfies the condition, it is 0 + */ + std::optional first_attention_stopline{std::nullopt}; + + /** + * second_attention_stopline is null if ego footprint along the path does not intersect with + * second_attention_lane. if path[0] satisfies the condition, it is 0 + */ + std::optional second_attention_stopline{std::nullopt}; + + /** + * occlusion_peeking_stopline is null if path[0] is already inside the attention area + */ + std::optional occlusion_peeking_stopline{std::nullopt}; + + /** + * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value + * is calculated negative, it is 0 + */ + size_t first_pass_judge_line{0}; + + /** + * 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 + */ + size_t second_pass_judge_line{0}; + + /** + * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with + * the centerline of the first_attention_lane + */ + size_t occlusion_wo_tl_pass_judge_line{0}; +}; +} // namespace behavior_velocity_planner::intersection + +#endif // INTERSECTION_STOPLINES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index c92f16dd7b474..615991bc5e027 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -35,11 +35,10 @@ using tier4_autoware_utils::getOrDeclareParameter; IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), occlusion_rtc_interface_( &node, "intersection_occlusion", - getOrDeclareParameter( - node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; @@ -61,6 +60,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); ip.common.delay_response_time = getOrDeclareParameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); ip.stuck_vehicle.turn_direction.left = getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); @@ -92,8 +93,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.collision_detection_hold_time"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); - ip.collision_detection.keep_detection_velocity_threshold = getOrDeclareParameter( - node, ns + ".collision_detection.keep_detection_velocity_threshold"); ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter(node, ns + ".collision_detection.velocity_profile.use_upstream"); ip.collision_detection.velocity_profile.minimum_upstream_velocity = getOrDeclareParameter( @@ -181,8 +180,6 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); // run occlusion detection only in the first intersection - // TODO(Mamoru Sobue): remove `enable_occlusion_detection` variable - const bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -212,8 +209,7 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, enable_occlusion_detection, node_, - logger_.get_child("intersection_module"), clock_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index ff9302db0b6af..88af4412e34eb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -44,7 +44,6 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC IntersectionModule::PlannerParam intersection_param_; // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - std::unordered_map occlusion_map_uuid_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp new file mode 100644 index 0000000000000..cc6ad880b8153 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -0,0 +1,44 @@ +// 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 RESULT_HPP_ +#define RESULT_HPP_ + +#include + +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) {} + explicit operator bool() const noexcept { return std::holds_alternative(data_); } + bool operator!() const noexcept { return !static_cast(*this); } + const Ok & ok() const { return std::get(data_); } + const Error & err() const { return std::get(data_); } + +private: + std::variant data_; +}; + +} // 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 dad0c194b5c9f..90242dc3edd7e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,137 +16,42 @@ #include "util.hpp" -#include -#include +#include // for toGeomPoly #include -#include -#include -#include #include #include -#include -#include +#include // for toPolygon2d #include #include -#include -#include -#include +#include -#include -#include #include #include -#include +#include +#include #include #include -#include -#include #include -#include #include -namespace tier4_autoware_utils -{ - -template <> -inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - point.z = p.z(); - return point; -} - -} // namespace tier4_autoware_utils - namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - 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); - - Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - -static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -static bool isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { - return true; - } - return false; -} - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), has_traffic_light_(has_traffic_light), - enable_occlusion_detection_(enable_occlusion_detection), - occlusion_attention_divisions_(std::nullopt), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -185,6 +90,32 @@ IntersectionModule::IntersectionModule( "~/debug/intersection/object_ttc", 1); } +bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + 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; + + const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + + intersection::formatDecisionResult(decision_result); + std_msgs::msg::String decision_result_msg; + decision_result_msg.data = decision_type; + decision_state_pub_->publish(decision_result_msg); + + prepareRTCStatus(decision_result, *path); + + reactRTCApproval(decision_result, path, stop_reason); + + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; +} + void IntersectionModule::initializeRTCStatus() { setSafe(true); @@ -196,6 +127,255 @@ void IntersectionModule::initializeRTCStatus() // activated_ and occlusion_activated_ must be set from manager's RTC callback } +intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + + const auto prepare_data = prepareIntersectionData(is_prioritized, path); + if (!prepare_data) { + return prepare_data.err(); + } + 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 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(); + } + + // if attention area is empty, collision/occlusion detection is impossible + if (!intersection_lanelets.first_attention_area()) { + return intersection::Indecisive{"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"}; + } + 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"}; + } + 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(); + + // 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 [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(); + } + [[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 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); + 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); + + 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_); + const bool has_collision_with_margin = + collision_state_machine_.getState() == StateMachine::State::STOP; + + if (is_prioritized) { + return intersection::FullyPrioritized{ + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + + // Safe + if (!is_occlusion_state && !has_collision_with_margin) { + return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // Only collision + if (!is_occlusion_state && has_collision_with_margin) { + return intersection::NonOccludedCollisionStop{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // Occluded + // occlusion_status is assured to be not NOT_OCCLUDED + 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( + first_attention_stopline_idx, + planner_param_.occlusion.temporal_stop_time_before_peeking, + temporal_stop_before_attention_state_machine_) + : 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"}; + } + return intersection::OccludedAbsenceTrafficLight{ + is_occlusion_cleared_with_margin, + has_collision_with_margin, + temporal_stop_before_attention_required, + closest_idx, + first_attention_stopline_idx, + occlusion_wo_tl_pass_judge_line_idx}; + } + // 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( + occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && + is_static_occlusion; + if (has_collision_with_margin) { + // if collision is detected, timeout is reset + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } else if (is_stuck_by_static_occlusion) { + static_occlusion_timeout_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); + } + const bool release_static_occlusion_stuck = + (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); + if (!has_collision_with_margin && release_static_occlusion_stuck) { + return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED + const double max_timeout = + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + + planner_param_.occlusion.occlusion_detection_hold_time; + const std::optional static_occlusion_timeout = + is_stuck_by_static_occlusion + ? std::make_optional( + max_timeout - static_occlusion_timeout_state_machine_.getDuration() - + occlusion_stop_state_machine_.getDuration()) + : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); + if (has_collision_with_margin) { + return intersection::OccludedCollisionStop{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; + } else { + return intersection::PeekingTowardOcclusion{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; + } + } else { + const auto occlusion_stopline = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? first_attention_stopline_idx + : occlusion_stopline_idx; + return intersection::FirstWaitBeforeOcclusion{ + is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; + } +} + // template-specification based visitor pattern // https://en.cppreference.com/w/cpp/utility/variant/visit template @@ -218,7 +398,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const IntersectionModule::Indecisive & result, + [[maybe_unused]] const intersection::Indecisive & 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) @@ -228,7 +408,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::StuckStop & result, + const intersection::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -248,7 +428,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::YieldStuckStop & result, + const intersection::YieldStuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -263,7 +443,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::NonOccludedCollisionStop & result, + const intersection::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -282,7 +462,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::FirstWaitBeforeOcclusion & result, + const intersection::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -301,7 +481,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::PeekingTowardOcclusion & result, + const intersection::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -320,7 +500,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::OccludedAbsenceTrafficLight & result, + const intersection::OccludedAbsenceTrafficLight & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -337,7 +517,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::OccludedCollisionStop & result, + const intersection::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -356,9 +536,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::Safe & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const intersection::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; @@ -375,7 +555,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::FullyPrioritized & result, + const intersection::FullyPrioritized & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -393,7 +573,7 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const DecisionResult & decision_result, + const intersection::DecisionResult & decision_result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; @@ -408,7 +588,7 @@ void IntersectionModule::prepareRTCStatus( setSafe(default_safety); setDistance(default_distance); occlusion_first_stop_required_ = - std::holds_alternative(decision_result); + std::holds_alternative(decision_result); } template @@ -416,7 +596,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -426,13 +606,13 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const IntersectionModule::Indecisive & decision_result, + [[maybe_unused]] const intersection::Indecisive & 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]] DebugData * debug_data) + [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; } @@ -440,10 +620,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::StuckStop & decision_result, + const intersection::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -488,10 +669,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::YieldStuckStop & decision_result, + const intersection::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -520,10 +702,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::NonOccludedCollisionStop & decision_result, + const intersection::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -563,10 +746,10 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::FirstWaitBeforeOcclusion & decision_result, + const intersection::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -614,10 +797,10 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::PeekingTowardOcclusion & decision_result, + const intersection::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -671,10 +854,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::OccludedCollisionStop & decision_result, + const intersection::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -718,10 +902,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, + const intersection::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -771,10 +956,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::Safe & decision_result, + const intersection::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -813,10 +999,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::FullyPrioritized & decision_result, + const intersection::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -853,96 +1040,26 @@ void reactRTCApprovalByDecisionResult( return; } -void reactRTCApproval( - const bool rtc_default_approval, const bool rtc_occlusion_approval, - const IntersectionModule::DecisionResult & decision_result, - const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) +void IntersectionModule::reactRTCApproval( + const intersection::DecisionResult & decision_result, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( - rtc_default_approval, rtc_occlusion_approval, decision, planner_param, baselink2front, path, - stop_reason, velocity_factor, debug_data); + activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, + stop_reason, &velocity_factor_, &debug_data_); }}, decision_result); return; } -static std::string formatDecisionResult(const IntersectionModule::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 "Safe"; - } - 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 "FullyPrioritized"; - } - return ""; -} - -bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - 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; - - const std::string decision_type = - "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); - std_msgs::msg::String decision_result_msg; - decision_result_msg.data = decision_type; - decision_state_pub_->publish(decision_result_msg); - - prepareRTCStatus(decision_result, *path); - - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - reactRTCApproval( - activated_, occlusion_activated_, decision_result, planner_param_, baselink2front, path, - stop_reason, &velocity_factor_, &debug_data_); - - if (!activated_ || !occlusion_activated_) { - is_go_out_ = false; - } else { - is_go_out_ = true; - } - RCLCPP_DEBUG(logger_, "===== plan end ====="); - return true; -} - -bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) +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()) { @@ -961,1384 +1078,55 @@ bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) } const auto & tl_info = tl_info_opt.value(); for (auto && tl_light : tl_info.signal.elements) { - if ( - tl_light.color == TrafficSignalElement::GREEN && - tl_light.shape == TrafficSignalElement::CIRCLE) { - return true; - } - } - return false; -} - -IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) -{ - 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_); - const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - const auto & current_pose = planner_data_->current_odometry->pose; - - // spline interpolation - 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 IntersectionModule::Indecisive{"splineInterpolate failed"}; - } - const auto & interpolated_path_info = interpolated_path_info_opt.value(); - if (!interpolated_path_info.lane_id_interval) { - return IntersectionModule::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}; - } - - // cache intersection lane information because it is invariant - const auto lanelets_on_path = - planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); - if (!intersection_lanelets_) { - intersection_lanelets_ = - getObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path); - } - 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 - const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); - const bool is_prioritized = - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update( - is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); - - // this is abnormal - const auto & conflicting_lanelets = intersection_lanelets.conflicting(); - const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); - const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); - if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { - return IntersectionModule::Indecisive{"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 - const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value() - : first_conflicting_lane; - - const auto intersection_stoplines_opt = generateIntersectionStopLines( - assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, - interpolated_path_info, path); - if (!intersection_stoplines_opt) { - return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; - } - const auto & intersection_stoplines = intersection_stoplines_opt.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; - const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; - const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; - const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; - const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; - const auto occlusion_wo_tl_pass_judge_line_idx = - intersection_stoplines.occlusion_wo_tl_pass_judge_line; - - // see the doc for struct PathLanelets - const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); - const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = generatePathLanelets( - 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 IntersectionModule::Indecisive{"failed to generate PathLanelets"}; - } - const auto & path_lanelets = path_lanelets_opt.value(); - - // 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 vehicle detection is viable even if attention area is empty - // so this needs to be checked before attention area validation - const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets, &debug_data_); - const bool is_first_conflicting_lane_private = - (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); - if (stuck_detected) { - if ( - is_first_conflicting_lane_private && - planner_param_.stuck_vehicle.disable_against_private_lane) { - // do nothing - } else { - std::optional stopline_idx = std::nullopt; - if (stuck_stopline_idx_opt) { - const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < - -planner_param_.common.stopline_overshoot_margin; - if (!is_over_stuck_stopline) { - stopline_idx = stuck_stopline_idx_opt.value(); - } - } - if (!stopline_idx) { - if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { - stopline_idx = default_stopline_idx_opt.value(); - } else if ( - first_attention_stopline_idx_opt && - fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { - stopline_idx = closest_idx; - } - } - if (stopline_idx) { - return IntersectionModule::StuckStop{ - closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; - } - } - } - - // if attention area is empty, collision/occlusion detection is impossible - if (!first_attention_area_opt) { - return IntersectionModule::Indecisive{"attention area is empty"}; - } - const auto first_attention_area = first_attention_area_opt.value(); - - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line - if (!default_stopline_idx_opt) { - return IntersectionModule::Indecisive{"default stop line is null"}; - } - // 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 - if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return IntersectionModule::Indecisive{"occlusion stop line is null"}; - } - const auto default_stopline_idx = default_stopline_idx_opt.value(); - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); - const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); - const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); - - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.occlusion_attention_area = occlusion_attention_area; - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - - // check occlusion on detection lane - if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = generateDetectionLaneDivisions( - occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution); - } - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); - - // get intersection area - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - - const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(), - &debug_data_); - if (yield_stuck_detected) { - std::optional stopline_idx = std::nullopt; - const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; - const bool is_before_first_attention_stopline = - fromEgoDist(first_attention_stopline_idx) >= 0.0; - if (stuck_stopline_idx_opt) { - const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < - -planner_param_.common.stopline_overshoot_margin; - if (!is_over_stuck_stopline) { - stopline_idx = stuck_stopline_idx_opt.value(); - } - } - if (!stopline_idx) { - if (is_before_default_stopline) { - stopline_idx = default_stopline_idx; - } else if (is_before_first_attention_stopline) { - stopline_idx = closest_idx; - } - } - if (stopline_idx) { - return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()}; - } - } - - const bool is_amber_or_red = - (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); - auto occlusion_status = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? getOcclusionStatus( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) - : OcclusionType::NOT_OCCLUDED; - occlusion_stop_state_machine_.setStateWithMarginTime( - occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, - logger_.get_child("occlusion_stop"), *clock_); - const bool is_occlusion_cleared_with_margin = - (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // distinguish if ego detected occlusion or RTC detects occlusion - const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - if (ext_occlusion_requested) { - occlusion_status = OcclusionType::RTC_OCCLUDED; - } - const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); - if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { - occlusion_status = prev_occlusion_status_; - } else { - prev_occlusion_status_ = occlusion_status; - } - - // TODO(Mamoru Sobue): this part needs more formal handling - const size_t pass_judge_line_idx = [=]() { - if (enable_occlusion_detection_) { - // if occlusion detection is enabled, pass_judge position is beyond the boundary of first - // attention area - if (has_traffic_light_) { - return occlusion_stopline_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 - return default_pass_judge_line_idx; - } - } - return default_pass_judge_line_idx; - }(); - debug_data_.pass_judge_wall_pose = - planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); - const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - const double vel_norm = std::hypot( - planner_data_->current_velocity->twist.linear.x, - planner_data_->current_velocity->twist.linear.y); - const bool keep_detection = - (vel_norm < planner_param_.collision_detection.keep_detection_velocity_threshold); - const bool was_safe = std::holds_alternative(prev_decision_result_); - // if ego is over the pass judge line and not stopped - if (is_over_default_stopline && !is_over_pass_judge_line && keep_detection) { - RCLCPP_DEBUG(logger_, "is_over_default_stopline && !is_over_pass_judge_line && keep_detection"); - // do nothing - } else if ( - (was_safe && is_over_default_stopline && is_over_pass_judge_line && is_go_out_) || - is_permanent_go_) { - // is_go_out_: previous RTC approval - // activated_: current RTC approval - is_permanent_go_ = true; - return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; - } - - // 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(assigned_lanelet); - if (is_green_solid_on) { - 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(); - } - } - if (initial_green_light_observed_time_) { - const auto now = clock_->now(); - 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; - }); - if ( - exist_close_vehicles && - rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < - planner_param_.collision_detection.yield_on_green_traffic_light.duration) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - } - } - - // calculate dynamic collision around attention area - const double time_to_restart = - (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, - std::min(occlusion_stopline_idx, path->points.size() - 1), 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_); - const bool has_collision_with_margin = - collision_state_machine_.getState() == StateMachine::State::STOP; - - if (is_prioritized) { - return FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - - // Safe - if (!is_occlusion_state && !has_collision_with_margin) { - return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // Only collision - if (!is_occlusion_state && has_collision_with_margin) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED - 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( - first_attention_stopline_idx, - planner_param_.occlusion.temporal_stop_time_before_peeking, - temporal_stop_before_attention_state_machine_) - : false; - if (!has_traffic_light_) { - if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return IntersectionModule::Indecisive{ - "already passed maximum peeking line in the absence of traffic light"}; - } - return IntersectionModule::OccludedAbsenceTrafficLight{ - is_occlusion_cleared_with_margin, - has_collision_with_margin, - temporal_stop_before_attention_required, - closest_idx, - first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; - } - // 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( - occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && - is_static_occlusion; - if (has_collision_with_margin) { - // if collision is detected, timeout is reset - static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); - } else if (is_stuck_by_static_occlusion) { - static_occlusion_timeout_state_machine_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); - } - const bool release_static_occlusion_stuck = - (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); - if (!has_collision_with_margin && release_static_occlusion_stuck) { - return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED - const double max_timeout = - planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + - planner_param_.occlusion.occlusion_detection_hold_time; - const std::optional static_occlusion_timeout = - is_stuck_by_static_occlusion - ? std::make_optional( - max_timeout - static_occlusion_timeout_state_machine_.getDuration() - - occlusion_stop_state_machine_.getDuration()) - : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); - if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{ - is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stopline_idx, - first_attention_stopline_idx, - occlusion_stopline_idx, - static_occlusion_timeout}; - } else { - return IntersectionModule::PeekingTowardOcclusion{ - is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stopline_idx, - first_attention_stopline_idx, - occlusion_stopline_idx, - static_occlusion_timeout}; - } - } else { - const auto occlusion_stopline = - (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) - ? first_attention_stopline_idx - : occlusion_stopline_idx; - return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; - } -} - -TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane) -{ - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - - 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 (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -IntersectionLanelets IntersectionModule::getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path) -{ - const double detection_area_length = planner_param_.common.attention_area_length; - const double occlusion_detection_area_length = - planner_param_.occlusion.occlusion_attention_area_length; - const bool consider_wrong_direction_vehicle = - planner_param_.collision_detection.consider_wrong_direction_vehicle; - - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - - // for low priority lane - // 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(); - for (const auto & right_of_way : right_of_ways) { - if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { - for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { - yield_lanelets.push_back(yield_lanelet); - for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { - yield_lanelets.push_back(previous_lanelet); - } - } - } - } - - // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets = lanelets_on_path; - for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { - ego_lanelets.push_back(previous_lanelet); - for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { - if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { - continue; - } - ego_lanelets.push_back(following_lanelet); - } - } - - // get conflicting lanes on assigned lanelet - const auto & conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); - std::vector adjacent_followings; - - for (const auto & conflicting_lanelet : conflicting_lanelets) { - for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - } - - // final objective lanelets - lanelet::ConstLanelets detection_lanelets; - lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stopline for stuck vehicle - for (auto && conflicting_lanelet : conflicting_lanelets) { - if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) - conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); - } - - // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction_ == std::string("straight") && has_traffic_light) { - // if assigned lanelet is "straight" with traffic light, detection area is not necessary - } else { - if (consider_wrong_direction_vehicle) { - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - for (const auto & adjacent_following : adjacent_followings) { - detection_lanelets.push_back(adjacent_following); - } - } else { - // otherwise we need to know the priority from RightOfWay - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if ( - lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || - lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - } - } - - // get possible lanelet path that reaches conflicting_lane longer than given length - lanelet::ConstLanelets detection_and_preceding_lanelets; - { - const double length = detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); - } - } - } - } - - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; - { - const double length = occlusion_detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); - } - } - } - } - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; - for (const auto & ll : occlusion_detection_and_preceding_lanelets) { - const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (turn_direction == "left" || turn_direction == "right") { - continue; - } - occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); - } - - auto [attention_lanelets, original_attention_lanelet_sequences] = - util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - - IntersectionLanelets result; - result.attention_ = std::move(attention_lanelets); - for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { - // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that - // back() exists. - std::optional stopline{std::nullopt}; - for (auto it = original_attention_lanelet_seq.rbegin(); - it != original_attention_lanelet_seq.rend(); ++it) { - const auto traffic_lights = it->regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - break; - } - if (stopline) break; - } - result.attention_stoplines_.push_back(stopline); - } - result.attention_non_preceding_ = std::move(detection_lanelets); - for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stopline = std::nullopt; - const auto & ll = result.attention_non_preceding_.at(i); - const auto traffic_lights = ll.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - } - result.attention_non_preceding_stoplines_.push_back(stopline); - } - result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); - // NOTE: occlusion_attention is not inverted here - // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and - // then trim part of them based on curvature threshold - result.occlusion_attention_ = - std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); - - // NOTE: to properly update(), each element in conflicting_/conflicting_area_, - // attention_non_preceding_/attention_non_preceding_area_ need to be matched - result.attention_area_ = getPolygon3dFromLanelets(result.attention_); - result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); - result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); - result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); - result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); - return result; -} - -std::optional IntersectionModule::generateIntersectionStopLines( - lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::ConstLanelet & first_attention_lane, - const std::optional & second_attention_area_opt, - const util::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; - const double stopline_margin = planner_param_.common.default_stopline_margin; - const double max_accel = planner_param_.common.max_accel; - const double max_jerk = planner_param_.common.max_jerk; - const double delay_response_time = planner_param_.common.delay_response_time; - const double peeking_offset = planner_param_.occlusion.peeking_offset; - - const auto first_attention_area = first_attention_lane.polygon3d(); - const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); - - // find the index of the first point whose vehicle footprint on it intersects with attention_area - const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - const std::optional first_footprint_inside_1st_attention_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_1st_attention_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_1st_attention_ip = - first_footprint_inside_1st_attention_ip_opt.value(); - - std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; - for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // NOTE: maybe consideration of braking dist is necessary - first_footprint_attention_centerline_ip_opt = i; - break; - } - } - if (!first_footprint_attention_centerline_ip_opt) { - return std::nullopt; - } - const size_t first_footprint_attention_centerline_ip = - first_footprint_attention_centerline_ip_opt.value(); - - // (1) default stop line position on interpolated path - bool default_stopline_valid = true; - int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = - getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); - map_stop_idx_ip) { - stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } - if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; - } - if (stop_idx_ip_int < 0) { - default_stopline_valid = false; - } - const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; - - // (2) ego front stop line position on interpolated path - const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - - // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); - bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the attention area, invalid - { - const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { - occlusion_peeking_line_valid = false; - } - } - if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = - first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); - } - const auto occlusion_peeking_line_ip = static_cast( - std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - - // (4) first attention stopline position on interpolated path - const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; - const bool first_attention_stopline_valid = true; - - // (5) 1st pass judge line position on interpolated path - const double velocity = planner_data_->current_velocity->twist.linear.x; - const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; - const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_accel, max_jerk, delay_response_time); - int first_pass_judge_ip_int = - static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); - const auto first_pass_judge_line_ip = static_cast( - std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( - 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); - - // (6) stuck vehicle stopline position on interpolated path - 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 = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stopline_idx_ip_opt) { - stuck_stopline_valid = false; - stuck_stopline_ip_int = 0; - } else { - stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; - } - } else { - stuck_stopline_ip_int = - std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); - } - if (stuck_stopline_ip_int < 0) { - stuck_stopline_valid = false; - } - const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); - - // (7) second attention stopline position on interpolated path - int second_attention_stopline_ip_int = -1; - bool second_attention_stopline_valid = false; - if (second_attention_area_opt) { - const auto & second_attention_area = second_attention_area_opt.value(); - std::optional first_footprint_inside_2nd_attention_ip_opt = - getFirstPointInsidePolygonByFootprint( - second_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (first_footprint_inside_2nd_attention_ip_opt) { - second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); - } - } - const auto second_attention_stopline_ip = - second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) - : 0; - - // (8) second pass judge lie position on interpolated path - int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); - const auto second_pass_judge_line_ip = - static_cast(std::max(second_pass_judge_ip_int, 0)); - - struct IntersectionStopLinesTemp - { - size_t closest_idx{0}; - size_t stuck_stopline{0}; - size_t default_stopline{0}; - size_t first_attention_stopline{0}; - size_t second_attention_stopline{0}; - size_t occlusion_peeking_stopline{0}; - size_t first_pass_judge_line{0}; - size_t second_pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; - }; - - IntersectionStopLinesTemp intersection_stoplines_temp; - std::list> stoplines = { - {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, - {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, - {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, - {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, - {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, - {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, - {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; - stoplines.sort( - [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stoplines) { - const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = util::insertPointIndex( - insert_point, original_path, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - if (!insert_idx) { - return std::nullopt; - } - *stop_idx = insert_idx.value(); - } - if ( - intersection_stoplines_temp.occlusion_peeking_stopline < - intersection_stoplines_temp.default_stopline) { - intersection_stoplines_temp.occlusion_peeking_stopline = - intersection_stoplines_temp.default_stopline; - } - - IntersectionStopLines intersection_stoplines; - intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; - if (stuck_stopline_valid) { - intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; - } - if (default_stopline_valid) { - intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; - } - if (first_attention_stopline_valid) { - intersection_stoplines.first_attention_stopline = - intersection_stoplines_temp.first_attention_stopline; - } - if (second_attention_stopline_valid) { - intersection_stoplines.second_attention_stopline = - intersection_stoplines_temp.second_attention_stopline; - } - if (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - 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; -} - -/** - * @brief Get stop point from map if exists - * @param stop_pose stop point defined on map - * @return true when the stop point is defined on map. - */ -std::optional IntersectionModule::getStopLineIndexFromMap( - const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - const auto road_markings = - assigned_lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stopline; - for (const auto & road_marking : road_markings) { - const std::string type = - road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); - if (type == lanelet::AttributeValueString::StopLine) { - stopline.push_back(road_marking->roadMarking()); - break; // only one stopline exists. - } - } - if (stopline.empty()) { - return std::nullopt; - } - - const auto p_start = stopline.front().front(); - const auto p_end = stopline.front().back(); - const LineString2d extended_stopline = - planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); - - for (size_t i = lane_interval.first; i < lane_interval.second; i++) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(extended_stopline, path_segment, collision_points); - - if (collision_points.empty()) { - continue; - } - - return i; - } - - geometry_msgs::msg::Pose stop_point_from_map; - stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); - stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); - stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - - return motion_utils::findFirstNearestIndexWithSoftConstraints( - path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); -} - -static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) -{ - lanelet::ConstLanelets previous_lanelets; - for (const auto & ll : lanelets_on_path) { - if (associative_ids.find(ll.id()) != associative_ids.end()) { - return previous_lanelets; - } - previous_lanelets.push_back(ll); - } - return previous_lanelets; -} - -// end inclusive -static lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - size_t prev_idx = start_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } - prev_idx = i; - const double yaw = tf2::getYaw(p.orientation); - const double x = p.position.x; - const double y = p.position.y; - // NOTE: maybe this is opposite - const double left_x = x + width / 2 * std::sin(yaw); - const double left_y = y - width / 2 * std::cos(yaw); - const double right_x = x - width / 2 * std::sin(yaw); - const double right_y = y + width / 2 * std::cos(yaw); - lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); - rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - -static std::optional> -getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, - const std::vector & polygons, const bool search_forward = true) -{ - if (search_forward) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - } - } else { - for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - if (i == 0) { - break; - } - } - } - return std::nullopt; -} - -std::optional IntersectionModule::generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx) -{ - const double width = planner_data_->vehicle_info_.vehicle_width_m; - static constexpr double path_lanelet_interval = 1.5; - - const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; - if (!assigned_lane_interval_opt) { - return std::nullopt; - } - const auto assigned_lane_interval = assigned_lane_interval_opt.value(); - const auto & path = interpolated_path_info.path; - - PathLanelets path_lanelets; - // prev - path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids_); - path_lanelets.all = path_lanelets.prev; - - // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; - if (closest_idx > assigned_lane_start) { - path_lanelets.all.push_back( - generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); - } - - // ego_or_entry2exit - const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); - path_lanelets.ego_or_entry2exit = - generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); - - // next - if (assigned_lane_end < path.points.size() - 1) { - const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); - const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.next.value()); - } - } - - const auto first_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); - const auto last_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) - : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); - if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { - const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); - const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, - path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); - if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); - } - } - return path_lanelets; -} - -bool IntersectionModule::checkStuckVehicleInIntersection( - const PathLanelets & path_lanelets, DebugData * debug_data) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - const bool stuck_detection_direction = [&]() { - return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || - (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || - (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); - }(); - if (!stuck_detection_direction) { - return false; - } - - const auto & objects_ptr = planner_data_->predicted_objects; - - // considering lane change in the intersection, these lanelets are generated from the path - const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; - Polygon2d stuck_vehicle_detect_area{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return false; - } - - double target_polygon_length = - getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); - lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; - if (path_lanelets.next) { - targets.push_back(path_lanelets.next.value()); - const double next_arc_length = - std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); - target_polygon_length += next_arc_length; - } - const auto target_polygon = - to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); - - if (target_polygon.empty()) { - return false; - } - - for (const auto & p : target_polygon) { - stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); - } - - stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); - bg::correct(stuck_vehicle_detect_area); - - debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { - continue; // not stop vehicle - } - - // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); - if (is_in_stuck_area && debug_data) { - debug_data->stuck_targets.objects.push_back(object); + if ( + tl_light.color == TrafficSignalElement::GREEN && + tl_light.shape == TrafficSignalElement::CIRCLE) { return true; } } return false; } -static lanelet::LineString3d getLineStringFromArcLength( - const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const { - lanelet::Points3d points; - double accumulated_length = 0; - size_t start_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s1) { - start_index = i; - break; - } - accumulated_length += length; - } - if (start_index < linestring.size() - 1) { - const auto & p1 = linestring[start_index]; - const auto & p2 = linestring[start_index + 1]; - const double residue = s1 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto start_basic_point = p1.basicPoint() + residue * direction_vector; - const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); - points.push_back(start_point); - } + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - accumulated_length = 0; - size_t end_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s2) { - end_index = i; - break; - } - accumulated_length += length; - } + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - for (size_t i = start_index + 1; i < end_index; i++) { - const auto p = lanelet::Point3d(linestring[i]); - points.push_back(p); + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; } - if (end_index < linestring.size() - 1) { - const auto & p1 = linestring[end_index]; - const auto & p2 = linestring[end_index + 1]; - const double residue = s2 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto end_basic_point = p1.basicPoint() + residue * direction_vector; - const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); - points.push_back(end_point); + if (!tl_id) { + // this lane has no traffic light + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - return lanelet::LineString3d{lanelet::InvalId, points}; -} - -static lanelet::ConstLanelet createLaneletFromArcLength( - const lanelet::ConstLanelet & lanelet, const double s1, const double s2) -{ - const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); - // make sure that s1, and s2 are between [0, lane_length] - const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); - const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); - - const auto ratio_s1 = s1_saturated / total_length; - const auto ratio_s2 = s2_saturated / total_length; - - const auto s1_left = - static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s2_left = - static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s1_right = - static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); - const auto s2_right = - static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); - - const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); - const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); - - return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); -} - -bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) -{ - const bool yield_stuck_detection_direction = [&]() { - return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || - (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || - (turn_direction_ == "straight" && planner_param_.yield_stuck.turn_direction.straight); - }(); - if (!yield_stuck_detection_direction) { - return false; + 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 double width = planner_data_->vehicle_info_.vehicle_width_m; - const double stuck_vehicle_vel_thr = - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; - const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; - - LineString2d sparse_intersection_path; - const auto [start, end] = interpolated_path_info.lane_id_interval.value(); - for (unsigned i = start; i < end; ++i) { - const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; - const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); - if (turn_direction_ == "right") { - const double right_x = point.x - width / 2 * std::sin(yaw); - const double right_y = point.y + width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(right_x, right_y); - } else if (turn_direction_ == "left") { - const double left_x = point.x + width / 2 * std::sin(yaw); - const double left_y = point.y - width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(left_x, left_y); - } else { - // straight - sparse_intersection_path.emplace_back(point.x, point.y); + 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; } - } - lanelet::ConstLanelets yield_stuck_detect_lanelets; - for (const auto & attention_lanelet : attention_lanelets) { - const auto centerline = attention_lanelet.centerline2d().basicLineString(); - std::vector intersects; - bg::intersection(sparse_intersection_path, centerline, intersects); - if (intersects.empty()) { - continue; + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; } - const auto intersect = intersects.front(); - const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( - centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); - const double yield_stuck_start = - std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); - const double yield_stuck_end = intersect_arc_coords.length; - yield_stuck_detect_lanelets.push_back( - createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); } - debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { - 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); - - 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); - if (is_in_lanelet) { - debug_data->yield_stuck_targets.objects.push_back(object.object); - return true; - } - } + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; } - return false; + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } TargetObjects IntersectionModule::generateTargetObjects( - const IntersectionLanelets & intersection_lanelets, + const intersection::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { const auto & objects_ptr = planner_data_->predicted_objects; @@ -2411,937 +1199,100 @@ TargetObjects IntersectionModule::generateTargetObjects( return target_objects; } -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const 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) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - - // check collision between target_objects predicted path and ego lane - // cut the predicted path at passing_time - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - ego_ttc_time_array.stamp = path.header.stamp; - ego_ttc_pub_->publish(ego_ttc_time_array); - } - - const double passing_time = time_distance_array.back().first; - cutPredictPathWithDuration(target_objects, passing_time); - - const auto & concat_lanelets = path_lanelets.all; - const auto closest_arc_coords = getArcCoordinates( - concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const auto & ego_lane = path_lanelets.ego_or_entry2exit; - debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - - // change TTC margin based on ego traffic light color - 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); - }(); - const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { - if (!target_object.dist_to_stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - if (dist_to_stopline < 0) { - return false; - } - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - return dist_to_stopline > braking_distance; - }; - const auto isTolerableOvershoot = [&](const TargetObject & target_object) { - if ( - !target_object.attention_lanelet || !target_object.dist_to_stopline || - !target_object.stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - if (dist_to_stopline > braking_distance) { - return false; - } - const auto stopline_front = target_object.stopline.value().front(); - const auto stopline_back = target_object.stopline.value().back(); - tier4_autoware_utils::LineString2d object_line; - object_line.emplace_back( - (stopline_front.x() + stopline_back.x()) / 2.0, - (stopline_front.y() + stopline_back.y()) / 2.0); - const auto stopline_mid = object_line.front(); - const auto endpoint = target_object.attention_lanelet.value().centerline().back(); - object_line.emplace_back(endpoint.x(), endpoint.y()); - std::vector intersections; - bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); - if (intersections.empty()) { - return false; - } - const auto collision_point = intersections.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_collision = - std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); - const double object2collision = stopline_to_collision - stopline_to_object; - const double margin = - planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; - return (object2collision > margin) || (object2collision < 0); - }; - // check collision between predicted_path and ego_area - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; - object_ttc_time_array.layout.dim.resize(3); - object_ttc_time_array.layout.dim.at(0).label = "objects"; - object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id - object_ttc_time_array.layout.dim.at(1).label = - "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " - "start_time, start_dist, " - "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " - "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; - object_ttc_time_array.layout.dim.at(1).size = 57; - for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { - object_ttc_time_array.data.push_back(lane_id_); - } - bool collision_detected = false; - for (const auto & target_object : target_objects->all_attention_objects) { - const auto & object = target_object.object; - // If the vehicle is expected to stop before their stopline, ignore - const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expected_to_stop_before_stopline) { - debug_data_.amber_ignore_targets.objects.push_back(object); - continue; - } - const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && - !expected_to_stop_before_stopline && is_tolerable_overshoot) { - debug_data_.red_overshoot_ignore_targets.objects.push_back(object); - continue; - } - for (const auto & predicted_path : object.kinematics.predicted_paths) { - if ( - predicted_path.confidence < - planner_param_.collision_detection.min_predicted_path_confidence) { - // ignore the predicted path with too low confidence - continue; - } - - // collision point - const auto first_itr = std::adjacent_find( - predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); - }); - if (first_itr == predicted_path.path.cend()) continue; - const auto last_itr = std::adjacent_find( - predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); - }); - if (last_itr == predicted_path.path.crend()) continue; - - // possible collision time interval - const double ref_object_enter_time = - static_cast(first_itr - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto start_time_distance_itr = time_distance_array.begin(); - if (ref_object_enter_time - collision_start_margin_time > 0) { - // start of possible ego position in the intersection - start_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_enter_time - collision_start_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (start_time_distance_itr == time_distance_array.end()) { - // ego is already at the exit of intersection when npc is at collision point even if npc - // accelerates so ego's position interval is empty - continue; - } - } - const double ref_object_exit_time = - static_cast(last_itr.base() - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto end_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_exit_time + collision_end_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (end_time_distance_itr == time_distance_array.end()) { - // ego is already passing the intersection, when npc is is at collision point - // so ego's position interval is up to the end of intersection lane - end_time_distance_itr = time_distance_array.end() - 1; - } - const double start_arc_length = std::max( - 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - - planner_data_->vehicle_info_.rear_overhang_m); - const double end_arc_length = std::min( - closest_arc_coords.length + (*end_time_distance_itr).second + - planner_data_->vehicle_info_.max_longitudinal_offset_m, - lanelet::utils::getLaneletLength2d(concat_lanelets)); - - const auto trimmed_ego_polygon = - getPolygonFromArcLength(concat_lanelets, start_arc_length, 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); - - for (auto itr = first_itr; itr != last_itr.base(); ++itr) { - const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); - if (bg::intersects(polygon, footprint_polygon)) { - collision_detected = true; - break; - } - } - object_ttc_time_array.layout.dim.at(0).size++; - const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto & shape = object.shape; - object_ttc_time_array.data.insert( - object_ttc_time_array.data.end(), - {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), - shape.dimensions.x, shape.dimensions.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x, - 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, - start_time_distance_itr->first, start_time_distance_itr->second, - end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, - first_itr->position.y, last_itr->position.x, last_itr->position.y}); - for (unsigned i = 0; i < 20; i++) { - const auto & pos = - predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; - object_ttc_time_array.data.push_back(pos.x); - object_ttc_time_array.data.push_back(pos.y); - } - if (collision_detected) { - debug_data_.conflicting_targets.objects.push_back(object); - break; - } - } - } - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - object_ttc_time_array.stamp = path.header.stamp; - object_ttc_pub_->publish(object_ttc_time_array); - } - - return collision_detected; -} - -std::optional IntersectionModule::checkAngleForTargetLanelets( - - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const bool is_parked_vehicle) const -{ - const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; - const bool consider_wrong_direction_vehicle = - planner_param_.common.attention_area_angle_threshold; - const double dist_margin = planner_param_.common.attention_area_margin; - - for (unsigned i = 0; i < target_lanelets.size(); ++i) { - const auto & ll = target_lanelets.at(i); - if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { - continue; - } - const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); - const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); - if (consider_wrong_direction_vehicle) { - if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - } else { - if (std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is - // positive - if ( - is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return std::make_optional(i); - } - } - } - return std::nullopt; -} - -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) +intersection::Result< + intersection::Indecisive, + std::pair> +IntersectionModule::isOverPassJudgeLinesStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const intersection::IntersectionStopLines & intersection_stoplines) { - 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); + const auto & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx = intersection_stoplines.closest_idx; + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + 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 + if (passed_1st_judge_line_while_peeking_) { + return occlusion_stopline_idx; } - } - } - } -} - -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, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - const double intersection_velocity = - planner_param_.collision_detection.velocity_profile.default_velocity; - const double minimum_ego_velocity = - planner_param_.collision_detection.velocity_profile.minimum_default_velocity; - const bool use_upstream_velocity = - planner_param_.collision_detection.velocity_profile.use_upstream; - const double minimum_upstream_velocity = - 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; - - // crop intersection part of the path, and set the reference velocity to intersection_velocity - // for ego's ttc - PathWithLaneId reference_path; - std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - auto reference_point = path.points.at(i); - // assume backward velocity is current ego velocity - if (i < closest_idx) { - reference_point.point.longitudinal_velocity_mps = current_velocity; - } - if ( - i > last_intersection_stopline_candidate_idx && - std::fabs(reference_point.point.longitudinal_velocity_mps) < - std::numeric_limits::epsilon() && - !upstream_stopline) { - upstream_stopline = i; - } - if (!use_upstream_velocity) { - reference_point.point.longitudinal_velocity_mps = intersection_velocity; - } - reference_path.points.push_back(reference_point); - bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); - if (assigned_lane_found && !has_objective_lane_id) { - break; - } - assigned_lane_found = has_objective_lane_id; - } - if (!assigned_lane_found) { - return {{0.0, 0.0}}; // has already passed the intersection. - } - - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - - // apply smoother to reference velocity - PathWithLaneId smoothed_reference_path = reference_path; - if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { - smoothed_reference_path = reference_path; - } - - // calculate when ego is going to reach each (interpolated) points on the path - TimeDistanceArray time_distance_array{}; - double dist_sum = 0.0; - double passing_time = time_delay; - time_distance_array.emplace_back(passing_time, dist_sum); - - // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); - - const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { - if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stopline_point, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); - } else { - return std::nullopt; - } - }(); - const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); - - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { - const auto & p1 = smoothed_reference_path.points.at(i); - const auto & p2 = smoothed_reference_path.points.at(i + 1); - - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); - dist_sum += dist; - - // use average velocity between p1 and p2 - const double average_velocity = - (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double passing_velocity = [=]() { - if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { - return minimum_upstream_velocity; + const bool is_over_first_pass_judge_line = + util::isOverTargetIndex(path, closest_idx, current_pose, first_pass_judge_line_idx); + 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; } - return std::max(average_velocity, minimum_ego_velocity); + } 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 { - return std::max(average_velocity, minimum_ego_velocity); - } - }(); - passing_time += (dist / passing_velocity); - - time_distance_array.emplace_back(passing_time, dist_sum); - } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); - for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id_); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); - } - return time_distance_array; -} - -static double getHighestCurvature(const lanelet::ConstLineString3d & centerline) -{ - std::vector points; - for (auto point = centerline.begin(); point != centerline.end(); point++) { - points.push_back(*point); - } - - SplineInterpolationPoints2d interpolation(points); - const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); - std::vector curvatures_positive; - for (const auto & curvature : curvatures) { - curvatures_positive.push_back(std::fabs(curvature)); - } - return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); -} - -std::vector IntersectionModule::generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) -{ - const double curvature_threshold = - planner_param_.occlusion.attention_lane_crop_curvature_threshold; - const double curvature_calculation_ds = - planner_param_.occlusion.attention_lane_curvature_calculation_ds; - - using lanelet::utils::getCenterlineWithOffset; - - // (0) remove left/right lanelet - lanelet::ConstLanelets detection_lanelets; - for (const auto & detection_lanelet : detection_lanelets_all) { - // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet - const auto fine_centerline = - lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); - const double highest_curvature = getHighestCurvature(fine_centerline); - if (highest_curvature > curvature_threshold) { - continue; - } - detection_lanelets.push_back(detection_lanelet); - } - - // (1) tsort detection_lanelets - const auto [merged_detection_lanelets, originals] = - util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); - - // (2) merge each branch to one lanelet - // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::vector> merged_lanelet_with_area; - for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { - const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); - const auto & original = originals.at(i); - double area = 0; - for (const auto & partition : original) { - area += bg::area(partition.polygon2d().basicPolygon()); - } - merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); - } - - // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { - const double length = bg::length(merged_lanelet.centerline()); - const double width = area / length; - for (int i = 0; i < static_cast(width / resolution); ++i) { - const double offset = resolution * i - width / 2; - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); - } - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); - } - return detection_divisions; -} - -IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) -{ - 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; - - const auto & path_ip = interpolated_path_info.path; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - - const auto first_attention_area_idx = - util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); - if (!first_attention_area_idx) { - return OcclusionType::NOT_OCCLUDED; - } - - const auto first_inside_attention_idx_ip_opt = - util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); - const std::pair lane_attention_interval_ip = - first_inside_attention_idx_ip_opt - ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) - : lane_interval_ip; - const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; - - const int width = occ_grid.info.width; - const int height = occ_grid.info.height; - const double resolution = occ_grid.info.resolution; - const auto & origin = occ_grid.info.origin.position; - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - - Polygon2d grid_poly; - grid_poly.outer().emplace_back(origin.x, origin.y); - grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); - grid_poly.outer().emplace_back( - origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution); - grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution); - grid_poly.outer().emplace_back(origin.x, origin.y); - bg::correct(grid_poly); - - auto findCommonCvPolygons = - [&](const auto & area2d, std::vector> & cv_polygons) -> void { - tier4_autoware_utils::Polygon2d area2d_poly; - for (const auto & p : area2d) { - area2d_poly.outer().emplace_back(p.x(), p.y()); - } - area2d_poly.outer().push_back(area2d_poly.outer().front()); - bg::correct(area2d_poly); - std::vector common_areas; - bg::intersection(area2d_poly, grid_poly, common_areas); - if (common_areas.empty()) { - return; - } - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - for (const auto & common_area : common_areas) { - std::vector cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); - cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - cv_polygons.push_back(cv_polygon); - } - }; - - // (1) prepare detection area mask - // attention: 255 - // non-attention: 0 - // NOTE: interesting area is set to 255 for later masking - cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); - std::vector> attention_area_cv_polygons; - for (const auto & attention_area : attention_areas) { - const auto area2d = lanelet::utils::to2D(attention_area); - findCommonCvPolygons(area2d, attention_area_cv_polygons); - } - for (const auto & poly : attention_area_cv_polygons) { - cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); - } - // (1.1) - // reset adjacent_lanelets area to 0 on attention_mask - std::vector> adjacent_lane_cv_polygons; - for (const auto & adjacent_lanelet : adjacent_lanelets) { - const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); - findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); - } - for (const auto & poly : adjacent_lane_cv_polygons) { - cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); - } - - // (2) prepare unknown mask - // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] - // unknown: 255 - // not-unknown: 0 - cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); - cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); - for (int x = 0; x < width; x++) { - for (int y = 0; y < height; y++) { - const int idx = y * width + x; - const unsigned char intensity = occ_grid.data.at(idx); - if ( - planner_param_.occlusion.free_space_max <= intensity && - intensity < planner_param_.occlusion.occupied_min) { - unknown_mask_raw.at(height - 1 - y, x) = 255; - } - } - } - // (2.1) apply morphologyEx - const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); - cv::morphologyEx( - unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); - - // (3) occlusion mask - static constexpr unsigned char OCCLUDED = 255; - static constexpr unsigned char BLOCKED = 127; - cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); - cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); - // 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); - } - 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); - findCommonCvPolygons(obj_poly.outer(), blocking_polygons); - } - for (const auto & blocking_polygon : blocking_polygons) { - cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); - } - for (const auto & division : lane_divisions) { - bool blocking_vehicle_found = false; - for (const auto & point_it : division) { - const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); - if (!valid) continue; - if (blocking_vehicle_found) { - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - continue; - } - if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { - blocking_vehicle_found = true; - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - } - } - } - - // (4) extract occlusion polygons - const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; - const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; - const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; - const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; - std::vector> contours; - cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); - std::vector> valid_contours; - for (const auto & contour : contours) { - if (contour.size() <= 2) { - continue; - } - std::vector approx_contour; - cv::approxPolyDP( - contour, approx_contour, - std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); - if (approx_contour.size() <= 2) continue; - // check area - const double poly_area = cv::contourArea(approx_contour); - if (poly_area < possible_object_area) continue; - // check bounding box size - const auto bbox = cv::minAreaRect(approx_contour); - if (const auto size = bbox.size; std::min(size.height, size.width) < - std::min(possible_object_bbox_x, possible_object_bbox_y) || - std::max(size.height, size.width) < - std::max(possible_object_bbox_x, possible_object_bbox_y)) { - continue; - } - valid_contours.push_back(approx_contour); - geometry_msgs::msg::Polygon polygon_msg; - geometry_msgs::msg::Point32 point_msg; - for (const auto & p : approx_contour) { - const double glob_x = (p.x + 0.5) * resolution + origin.x; - const double glob_y = (height - 0.5 - p.y) * resolution + origin.y; - point_msg.x = glob_x; - point_msg.y = glob_y; - point_msg.z = origin.z; - polygon_msg.points.push_back(point_msg); - } - debug_data_.occlusion_polygons.push_back(polygon_msg); - } - // (4.1) re-draw occluded cells using valid_contours - occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - for (const auto & valid_contour : valid_contours) { - // NOTE: drawContour does not work well - cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); - } - - // (5) find distance - // (5.1) discretize path_ip with resolution for computational cost - LineString2d path_linestring; - path_linestring.emplace_back( - path_ip.points.at(lane_start_idx).point.pose.position.x, - path_ip.points.at(lane_start_idx).point.pose.position.y); - { - auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; - for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { - const auto path_linestring_point = path_ip.points.at(i).point.pose.position; - if ( - tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < - 1.0 /* rough tick for computational cost */) { - continue; - } - path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); - prev_path_linestring_point = path_linestring_point; - } - } - - auto findNearestPointToProjection = []( - const lanelet::ConstLineString3d & division, - const Point2d & projection, const double dist_thresh) { - double min_dist = std::numeric_limits::infinity(); - auto nearest = division.end(); - for (auto it = division.begin(); it != division.end(); it++) { - const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); - if (dist < min_dist) { - min_dist = dist; - nearest = it; - } - if (dist < dist_thresh) { - break; - } - } - return nearest; - }; - struct NearestOcclusionPoint - { - int64 division_index{0}; - int64 point_index{0}; - double dist{0.0}; - geometry_msgs::msg::Point point; - geometry_msgs::msg::Point projection; - } nearest_occlusion_point; - double min_dist = std::numeric_limits::infinity(); - for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { - const auto & division = lane_divisions.at(division_index); - LineString2d division_linestring; - auto division_point_it = division.begin(); - division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); - for (auto point_it = division.begin(); point_it != division.end(); point_it++) { - if ( - std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < - 3.0 /* rough tick for computational cost */) { - continue; - } - division_linestring.emplace_back(point_it->x(), point_it->y()); - division_point_it = point_it; - } - - // find the intersection point of lane_line and path - std::vector intersection_points; - boost::geometry::intersection(division_linestring, path_linestring, intersection_points); - if (intersection_points.empty()) { - continue; - } - const auto & projection_point = intersection_points.at(0); - const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); - if (projection_it == division.end()) { - continue; - } - double acc_dist = 0.0; - auto acc_dist_it = projection_it; - for (auto point_it = projection_it; point_it != division.end(); point_it++) { - const double dist = - std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); - acc_dist += dist; - acc_dist_it = point_it; - const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - if (!valid) continue; - const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); - if (pixel == BLOCKED) { - break; - } - if (pixel == OCCLUDED) { - if (acc_dist < min_dist) { - min_dist = acc_dist; - nearest_occlusion_point = { - division_index, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; - } + // if there is no traffic light and occlusion is not detected, pass_judge position is + // default + return first_pass_judge_line_idx; } } - } - - if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return OcclusionType::NOT_OCCLUDED; - } + return first_pass_judge_line_idx; + }(); - debug_data_.nearest_occlusion_projection = - std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - 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); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - return OcclusionType::STATICALLY_OCCLUDED; -} + const bool was_safe = std::holds_alternative(prev_decision_result_); -static std::optional> -getFirstPointInsidePolygonsByFootprint( - const std::vector & polygons, - const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (size_t j = 0; j < polygons.size(); ++j) { - const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); - const bool is_in_polygon = bg::intersects(area_2d, path_footprint); - if (is_in_polygon) { - return std::make_optional>(i, j); - } - } + const bool is_over_1st_pass_judge_line = + util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); + if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { + safely_passed_1st_judge_line_time_ = clock_->now(); } - return std::nullopt; -} + 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(); + } + 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(); -void IntersectionLanelets::update( - const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, - lanelet::routing::RoutingGraphPtr routing_graph_ptr) -{ - is_prioritized_ = is_prioritized; - // find the first conflicting/detection area polygon intersecting the path - if (!first_conflicting_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - conflicting_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_conflicting_lane_ = conflicting_.at(first.value().second); - first_conflicting_area_ = conflicting_area_.at(first.value().second); - } - } - if (!first_attention_area_) { - const auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_attention_lane_ = attention_non_preceding_.at(first.value().second); - first_attention_area_ = attention_non_preceding_area_.at(first.value().second); - } - } - if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { - const auto first_attention_lane = first_attention_lane_.value(); - // remove first_attention_area_ and non-straight lanelets from attention_non_preceding - lanelet::ConstLanelets attention_non_preceding_ex_first; - lanelet::ConstLanelets sibling_first_attention_lanelets; - for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { - for (const auto & following : routing_graph_ptr->following(previous)) { - sibling_first_attention_lanelets.push_back(following); - } - } - for (const auto & ll : attention_non_preceding_) { - // the sibling lanelets of first_attention_lanelet are ruled out - if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { - continue; - } - if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { - attention_non_preceding_ex_first.push_back(ll); - } - } - if (attention_non_preceding_ex_first.empty()) { - second_attention_lane_empty_ = true; - } - const auto attention_non_preceding_ex_first_area = - getPolygon3dFromLanelets(attention_non_preceding_ex_first); - const auto second = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); - if (second) { - second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); - second_attention_area_ = second_attention_lane_.value().polygon3d(); - } + const bool is_over_default_stopline = + util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); + if ( + ((is_over_default_stopline || + planner_param_.common.enable_pass_judge_before_default_stopline) && + is_over_2nd_pass_judge_line && 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 + */ + 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() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 38feada725872..8a34aabe8b918 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -15,7 +15,11 @@ #ifndef SCENE_INTERSECTION_HPP_ #define SCENE_INTERSECTION_HPP_ -#include "util_type.hpp" +#include "decision_result.hpp" +#include "interpolated_path_info.hpp" +#include "intersection_lanelets.hpp" +#include "intersection_stoplines.hpp" +#include "result.hpp" #include #include @@ -26,268 +30,21 @@ #include #include -#include -#include +#include +#include +#include -#include -#include #include +#include #include #include +#include #include -#include #include namespace behavior_velocity_planner { -using TimeDistanceArray = std::vector>; - -struct DebugData -{ - std::optional collision_stop_wall_pose{std::nullopt}; - std::optional occlusion_stop_wall_pose{std::nullopt}; - std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional pass_judge_wall_pose{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 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 stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_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}; -}; - -/** - * @struct - * @brief see the document for more details of IntersectionLanelets - */ -struct IntersectionLanelets -{ -public: - /** - * update conflicting lanelets and traffic priority information - */ - void update( - const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, - lanelet::routing::RoutingGraphPtr routing_graph_ptr); - - const lanelet::ConstLanelets & attention() const - { - return is_prioritized_ ? attention_non_preceding_ : attention_; - } - const std::vector> & attention_stoplines() const - { - return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; - } - const lanelet::ConstLanelets & conflicting() const { return conflicting_; } - const lanelet::ConstLanelets & adjacent() const { return adjacent_; } - const lanelet::ConstLanelets & occlusion_attention() const - { - return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; - } - const lanelet::ConstLanelets & attention_non_preceding() const - { - return attention_non_preceding_; - } - const std::vector & attention_area() const - { - return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; - } - const std::vector & conflicting_area() const - { - return conflicting_area_; - } - const std::vector & adjacent_area() const { return adjacent_area_; } - const std::vector & occlusion_attention_area() const - { - return occlusion_attention_area_; - } - const std::optional & first_conflicting_lane() const - { - return first_conflicting_lane_; - } - const std::optional & first_conflicting_area() const - { - return first_conflicting_area_; - } - const std::optional & first_attention_lane() const - { - return first_attention_lane_; - } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } - const std::optional & second_attention_lane() const - { - return second_attention_lane_; - } - const std::optional & second_attention_area() const - { - return second_attention_area_; - } - - /** - * the set of attention lanelets which is topologically merged - */ - lanelet::ConstLanelets attention_; - std::vector attention_area_; - - /** - * the stop lines for each attention_ lanelets - */ - std::vector> attention_stoplines_; - - /** - * the conflicting part of attention lanelets - */ - lanelet::ConstLanelets attention_non_preceding_; - std::vector attention_non_preceding_area_; - - /** - * the stop lines for each attention_non_preceding_ - */ - std::vector> attention_non_preceding_stoplines_; - - /** - * the conflicting lanelets of the objective intersection lanelet - */ - lanelet::ConstLanelets conflicting_; - std::vector conflicting_area_; - - /** - * - */ - lanelet::ConstLanelets adjacent_; - std::vector adjacent_area_; - - /** - * the set of attention lanelets for occlusion detection which is topologically merged - */ - lanelet::ConstLanelets occlusion_attention_; - std::vector occlusion_attention_area_; - - /** - * the vector of sum of each occlusion_attention lanelet - */ - std::vector occlusion_attention_size_; - - /** - * the first conflicting lanelet which ego path points intersect for the first time - */ - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - - /** - * the first attention lanelet which ego path points intersect for the first time - */ - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - - /** - * the second attention lanelet which ego path points intersect next to the - * first_attention_lanelet - */ - bool second_attention_lane_empty_{false}; - std::optional second_attention_lane_{std::nullopt}; - std::optional second_attention_area_{std::nullopt}; - - /** - * flag if the intersection is prioritized by the traffic light - */ - bool is_prioritized_{false}; -}; - -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct IntersectionStopLines -{ - size_t closest_idx{0}; - - /** - * stuck_stopline is null if ego path does not intersect with first_conflicting_area - */ - std::optional stuck_stopline{std::nullopt}; - - /** - * default_stopline is null if it is calculated negative from first_attention_stopline - */ - std::optional default_stopline{std::nullopt}; - - /** - * first_attention_stopline is null if ego footprint along the path does not intersect with - * attention area. if path[0] satisfies the condition, it is 0 - */ - std::optional first_attention_stopline{std::nullopt}; - - /** - * second_attention_stopline is null if ego footprint along the path does not intersect with - * second_attention_lane. if path[0] satisfies the condition, it is 0 - */ - std::optional second_attention_stopline{std::nullopt}; - - /** - * occlusion_peeking_stopline is null if path[0] is already inside the attention area - */ - std::optional occlusion_peeking_stopline{std::nullopt}; - - /** - * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value - * is calculated negative, it is 0 - */ - size_t first_pass_judge_line{0}; - - /** - * second_pass_judge_line is before second_attention_stopline by the braking distance. if its - * value is calculated negative, it is 0 - */ - size_t second_pass_judge_line{0}; - - /** - * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with - * the centerline of the first_attention_lane - */ - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -/** - * @struct - * @brief see the document for more details of PathLanelets - */ -struct PathLanelets -{ - lanelet::ConstLanelets prev; - // lanelet::ConstLanelet entry2ego; this is included in `all` if exists - lanelet::ConstLanelet - ego_or_entry2exit; // this is `assigned lane` part of the path(not from - // ego) if ego is before the intersection, otherwise from ego to exit - std::optional next = - std::nullopt; // this is nullopt is the goal is inside intersection - lanelet::ConstLanelets all; - lanelet::ConstLanelets - conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the - // path -}; - /** * @struct * @brief see the document for more details of IntersectionStopLines @@ -314,19 +71,6 @@ struct TargetObjects std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy }; -/** - * @struct - * @brief categorize traffic light priority - */ -enum class TrafficPrioritizedLevel { - //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. - FULLY_PRIORITIZED = 0, - //! The target lane's traffic signal is amber - PARTIALLY_PRIORITIZED, - //! The target lane's traffic signal is green - NOT_PRIORITIZED -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -344,6 +88,7 @@ class IntersectionModule : public SceneModuleInterface double max_accel; double max_jerk; double delay_response_time; + bool enable_pass_judge_before_default_stopline; } common; struct TurnDirection @@ -373,7 +118,6 @@ class IntersectionModule : public SceneModuleInterface bool consider_wrong_direction_vehicle; double collision_detection_hold_time; double min_predicted_path_confidence; - double keep_detection_velocity_threshold; struct VelocityProfile { bool use_upstream; @@ -454,144 +198,77 @@ class IntersectionModule : public SceneModuleInterface RTC_OCCLUDED, }; - /** - * @struct - * @brief Internal error or ego already passed pass_judge_line - */ - struct Indecisive + struct DebugData { - std::string error; - }; - /** - * @struct - * @brief detected stuck vehicle - */ - struct StuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - std::optional occlusion_stopline_idx{std::nullopt}; - }; - /** - * @struct - * @brief yielded by vehicle on the attention area - */ - struct YieldStuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - }; - /** - * @struct - * @brief only collision is detected - */ - struct NonOccludedCollisionStop - { - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - }; - /** - * @struct - * @brief occlusion is detected so ego needs to stop at the default stop line position - */ - struct FirstWaitBeforeOcclusion - { - bool is_actually_occlusion_cleared{false}; - size_t closest_idx{0}; - size_t first_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - }; - /** - * @struct - * @brief ego is approaching the boundary of attention area in the presence of traffic light - */ - struct PeekingTowardOcclusion - { - //! if intersection_occlusion is disapproved externally through RTC, it indicates - //! "is_forcefully_occluded" - bool is_actually_occlusion_cleared{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t first_attention_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - //! 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 and shows up - //! intersection_occlusion(x.y) - std::optional static_occlusion_timeout{std::nullopt}; - }; - /** - * @struct - * @brief both collision and occlusion are detected in the presence of traffic light - */ - struct OccludedCollisionStop - { - bool is_actually_occlusion_cleared{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t first_attention_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - //! 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}; - }; - /** - * @struct - * @brief at least occlusion is detected in the absence of traffic light - */ - struct OccludedAbsenceTrafficLight - { - bool is_actually_occlusion_cleared{false}; - bool collision_detected{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t first_attention_area_stopline_idx{0}; - size_t peeking_limit_line_idx{0}; - }; - /** - * @struct - * @brief both collision and occlusion are not detected - */ - struct Safe - { - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; + std::optional collision_stop_wall_pose{std::nullopt}; + 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}; + bool passed_first_pass_judge{false}; + bool passed_second_pass_judge{false}; + std::optional second_pass_judge_wall_pose{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 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 stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_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}; }; + + using TimeDistanceArray = std::vector>; + /** * @struct - * @brief traffic light is red or arrow signal - */ - struct FullyPrioritized - { - bool collision_detected{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; + * @brief categorize traffic light priority + */ + enum class TrafficPrioritizedLevel { + //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + //! The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + //! The target lane's traffic signal is green + NOT_PRIORITIZED }; - using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line - StuckStop, //! detected stuck vehicle - YieldStuckStop, //! detected yield stuck vehicle - NonOccludedCollisionStop, //! detected collision while FOV is clear - FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion - PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected - OccludedCollisionStop, //! occlusion and collision are both detected - OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light - Safe, //! judge as safe - FullyPrioritized //! only detect vehicles violating traffic rules - >; + /** @} */ 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, - const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); - + const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup primary-function [fn] primary functions + * the entrypoint of this module is modifyPathVelocity() function that calculates safety decision + * of latest context and send it to RTC and then react to RTC approval. The reaction to RTC + * approval may not be based on the latest decision of this module depending on the auto-mode + * configuration. For module side it is not visible if the module is operating in auto-mode or + * manual-module. At first, initializeRTCStatus() is called to reset the safety value of + * INTERSECTION and INTERSECTION_OCCLUSION. Then modifyPathVelocityDetail() is called to analyze + * the context. Then prepareRTCStatus() is called to set the safety value of INTERSECTION and + * INTERSECTION_OCCLUSION. + * @{ + */ bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; @@ -606,134 +283,421 @@ class IntersectionModule : public SceneModuleInterface private: rclcpp::Node & node_; - const int64_t lane_id_; + + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup const-variables [var] const variables + * following variables are unique to this intersection lanelet or to this module + * @{ + */ + //! lanelet of this intersection + const lanelet::Id lane_id_; + + //! associative(sibling) lanelets ids const std::set associative_ids_; + + //! turn_direction of this lane const std::string turn_direction_; + + //! flag if this intersection is traffic controlled const bool has_traffic_light_; - // Parameter + //! RTC uuid for INTERSECTION_OCCLUSION + const UUID occlusion_uuid_; + /** @}*/ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup semi-const-variables [var] semi-const variables + * following variables are immutable once initialized + * @{ + */ PlannerParam planner_param_; - std::optional intersection_lanelets_{std::nullopt}; + //! cache IntersectionLanelets struct + std::optional intersection_lanelets_{std::nullopt}; + + //! cache discretized occlusion detection lanelets + std::optional> occlusion_attention_divisions_{ + std::nullopt}; + /** @}*/ - // for pass judge decision - bool is_go_out_{false}; +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup pass-judge-variable [var] pass judge variables + * following variables are state variables that depends on how the vehicle passed the intersection + * @{ + */ + //! if true, this module never commands to STOP anymore bool is_permanent_go_{false}; - DecisionResult prev_decision_result_{Indecisive{""}}; + + //! for checking if ego is over the pass judge lines because previously the situation was SAFE + intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + + //! 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}; + /** @}*/ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-variables [var] collision detection + * @{ + */ + //! debouncing for stable SAFE decision + StateMachine collision_state_machine_; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup occlusion-variables [var] occlusion detection variables + * @{ + */ OcclusionType prev_occlusion_status_; - // for occlusion detection - const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_{ - std::nullopt}; //! for caching discretized occlusion detection lanelets - StateMachine collision_state_machine_; //! for stable collision checking - StateMachine before_creep_state_machine_; //! for two phase stop - StateMachine occlusion_stop_state_machine_; //! for stable occlusion detection + //! debouncing for the first brief stop at the default stopline + StateMachine before_creep_state_machine_; + + //! debouncing for stable CLEARED decision + StateMachine occlusion_stop_state_machine_; + + //! debouncing for the brief stop at the boundary of attention area(if required by the flag) StateMachine temporal_stop_before_attention_state_machine_; + + //! time counter for the stuck detection due to occlusion caused static objects StateMachine static_occlusion_timeout_state_machine_; + /** @} */ std::optional initial_green_light_observed_time_{std::nullopt}; - // for RTC - const UUID occlusion_uuid_; +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup RTC-variables [var] RTC variables + * + * intersection module has additional rtc_interface_ for INTERSECTION_OCCLUSION in addition to the + * default rtc_interface of SceneModuleManagerInterfaceWithRTC. activated_ is the derived member + * of this module which is updated by the RTC config/service, so it should be read-only in this + * module. occlusion_safety_ and occlusion_stop_distance_ are the corresponding RTC value for + * INTERSECTION_OCCLUSION. + * @{ + */ bool occlusion_safety_{true}; double occlusion_stop_distance_{0.0}; bool occlusion_activated_{true}; bool occlusion_first_stop_required_{false}; + /** @}*/ +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @ingroup primary-functions + * @{ + */ /** - * @fn - * @brief set all RTC variable to safe and -inf + * @brief set all RTC variable to true(safe) and -INF */ void initializeRTCStatus(); + /** - * @fn * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + intersection::DecisionResult modifyPathVelocityDetail( + PathWithLaneId * path, StopReason * stop_reason); + /** - * @fn * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const DecisionResult &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const intersection::DecisionResult &, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); /** - * @fn - * @brief find TrafficPrioritizedLevel + * @brief act based on current RTC approval */ - TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane); + void reactRTCApproval( + const intersection::DecisionResult & decision_result, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + /** @}*/ +private: /** - * @fn - * @brief generate IntersectionLanelets + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup prepare-data [fn] basic data construction + * @{ */ - IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path); + /** + * @struct + */ + struct BasicData + { + intersection::InterpolatedPathInfo interpolated_path_info; + intersection::IntersectionStopLines intersection_stoplines; + intersection::PathLanelets path_lanelets; + }; + + /** + * @brief prepare basic data structure + * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @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( + const bool is_prioritized, PathWithLaneId * path); + + /** + * @brief find the associated stopline road marking of assigned lanelet + */ + std::optional getStopLineIndexFromMap( + const intersection::InterpolatedPathInfo & interpolated_path_info, + lanelet::ConstLanelet assigned_lanelet); /** - * @fn * @brief generate IntersectionStopLines */ - std::optional generateIntersectionStopLines( + std::optional generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); /** - * @fn - * @brief find the associated stopline road marking of assigned lanelet + * @brief generate IntersectionLanelets */ - std::optional getStopLineIndexFromMap( - const util::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet); + intersection::IntersectionLanelets generateObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet); /** - * @fn * @brief generate PathLanelets */ - std::optional generatePathLanelets( + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, const std::vector & attention_areas, const size_t closest_idx); /** - * @fn - * @brief check stuck + * @brief generate discretized detection lane linestring. */ - bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data); + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + /** @} */ +private: /** - * @fn - * @brief check yield stuck + * @defgroup utility [fn] utility member function + * @{ */ - bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data); + void stoppedAtPositionForDuration( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, + const double duration, StateMachine * state_machine); + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup get-traffic-light [fn] traffic light + * @{ + */ + /** + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn() const; + + /** + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; + /** @} */ +private: /** - * @fn * @brief categorize target objects */ TargetObjects generateTargetObjects( - const IntersectionLanelets & intersection_lanelets, + const intersection::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup yield [fn] check stuck + * @{ + */ + /** + * @brief check stuck status + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() + */ + std::optional isStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::IntersectionStopLines & intersection_stoplines, + const intersection::PathLanelets & path_lanelets) const; + + bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief check stuck + */ + bool checkStuckVehicleInIntersection(const intersection::PathLanelets & path_lanelets) const; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup yield [fn] check yield stuck + * @{ + */ + /** + * @brief check yield stuck status + * @attention this function has access to value() of intersection_lanelets_, + * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline + */ + 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; + + /** + * @brief check yield stuck + */ + bool checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) const; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup occlusion [fn] check occlusion + * @{ + */ + /** + * @brief check occlusion status + * @attention this function has access to value() of occlusion_attention_divisions_, + * 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); + + /** + * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + */ + 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); + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup pass-judge-decision [fn] pass judge decision + * @{ + */ + /** + * @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 + * (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( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const intersection::IntersectionStopLines & intersection_stoplines); + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-detection [fn] check collision + * @{ + */ + bool isTargetCollisionVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief check if there are any objects around the stoplines on the attention areas when ego + * entered the intersection on green light + * @return return NonOccludedCollisionStop if there are vehicle within the margin for some + * duration from ego's entry to yield + * @attention this function has access to value() of + * 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); + /** - * @fn * @brief check collision */ bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const PathLanelets & path_lanelets, const size_t closest_idx, + 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); @@ -744,7 +708,6 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); /** - * @fn * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position */ @@ -752,28 +715,7 @@ class IntersectionModule : public SceneModuleInterface 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, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - - std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); - - /** - * @fn - * @brief check occlusion status - */ - OcclusionType getOcclusionStatus( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); - - /* - * @fn - * @brief check if associated traffic light is green - */ - bool isGreenSolidOn(lanelet::ConstLanelet lane); + /** @} */ /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -783,7 +725,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - DebugData debug_data_; + mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_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 new file mode 100644 index 0000000000000..43bb68cf56f67 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -0,0 +1,582 @@ +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include // for toGeomPoly +#include // for smoothPath +#include +#include +#include // for toPolygon2d +#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; + +bool IntersectionModule::isTargetCollisionVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { + return true; + } + 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) +{ + 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 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(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool still_wait = + (rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration); + if (!still_wait) { + return std::nullopt; + } + 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; + }); + if (exist_close_vehicles) { + return intersection::NonOccludedCollisionStop{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + } + return std::nullopt; +} + +bool IntersectionModule::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) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getPolygonFromArcLength; + + // check collision between target_objects predicted path and ego lane + // cut the predicted path at passing_time + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = calcIntersectionPassingTime( + path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + ego_ttc_time_array.stamp = path.header.stamp; + ego_ttc_pub_->publish(ego_ttc_time_array); + } + + const double passing_time = time_distance_array.back().first; + cutPredictPathWithDuration(target_objects, passing_time); + + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = getArcCoordinates( + concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // change TTC margin based on ego traffic light color + 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); + }(); + const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { + if (!target_object.dist_to_stopline) { + return false; + } + const double dist_to_stopline = target_object.dist_to_stopline.value(); + if (dist_to_stopline < 0) { + return false; + } + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + return dist_to_stopline > braking_distance; + }; + const auto isTolerableOvershoot = [&](const TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stopline || + !target_object.stopline) { + return false; + } + const double dist_to_stopline = target_object.dist_to_stopline.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto stopline_front = target_object.stopline.value().front(); + const auto stopline_back = target_object.stopline.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.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_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; + // check collision between predicted_path and ego_area + tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + object_ttc_time_array.layout.dim.resize(3); + object_ttc_time_array.layout.dim.at(0).label = "objects"; + object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id + object_ttc_time_array.layout.dim.at(1).label = + "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " + "start_time, start_dist, " + "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " + "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array.layout.dim.at(1).size = 57; + for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { + object_ttc_time_array.data.push_back(lane_id_); + } + bool collision_detected = false; + for (const auto & target_object : target_objects->all_attention_objects) { + const auto & object = target_object.object; + // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + expected_to_stop_before_stopline) { + debug_data_.amber_ignore_targets.objects.push_back(object); + continue; + } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } + for (const auto & predicted_path : object.kinematics.predicted_paths) { + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + // ignore the predicted path with too low confidence + continue; + } + + // collision point + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); + }); + if (first_itr == predicted_path.path.cend()) continue; + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); + }); + if (last_itr == predicted_path.path.crend()) continue; + + // possible collision time interval + const double ref_object_enter_time = + static_cast(first_itr - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto start_time_distance_itr = time_distance_array.begin(); + if (ref_object_enter_time - collision_start_margin_time > 0) { + // start of possible ego position in the intersection + start_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (start_time_distance_itr == time_distance_array.end()) { + // ego is already at the exit of intersection when npc is at collision point even if npc + // accelerates so ego's position interval is empty + continue; + } + } + const double ref_object_exit_time = + static_cast(last_itr.base() - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto end_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (end_time_distance_itr == time_distance_array.end()) { + // ego is already passing the intersection, when npc is is at collision point + // so ego's position interval is up to the end of intersection lane + end_time_distance_itr = time_distance_array.end() - 1; + } + const double start_arc_length = std::max( + 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - + planner_data_->vehicle_info_.rear_overhang_m); + const double end_arc_length = std::min( + closest_arc_coords.length + (*end_time_distance_itr).second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + + const auto trimmed_ego_polygon = + getPolygonFromArcLength(concat_lanelets, start_arc_length, 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); + + for (auto itr = first_itr; itr != last_itr.base(); ++itr) { + const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); + if (bg::intersects(polygon, footprint_polygon)) { + collision_detected = true; + break; + } + } + object_ttc_time_array.layout.dim.at(0).size++; + const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & shape = object.shape; + object_ttc_time_array.data.insert( + object_ttc_time_array.data.end(), + {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), + shape.dimensions.x, shape.dimensions.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x, + 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, + start_time_distance_itr->first, start_time_distance_itr->second, + end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, + first_itr->position.y, last_itr->position.x, last_itr->position.y}); + for (unsigned i = 0; i < 20; i++) { + const auto & pos = + predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; + object_ttc_time_array.data.push_back(pos.x); + object_ttc_time_array.data.push_back(pos.y); + } + if (collision_detected) { + debug_data_.conflicting_targets.objects.push_back(object); + break; + } + } + } + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + object_ttc_time_array.stamp = path.header.stamp; + object_ttc_pub_->publish(object_ttc_time_array); + } + + return collision_detected; +} + +std::optional IntersectionModule::checkAngleForTargetLanelets( + + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const +{ + const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; + const bool consider_wrong_direction_vehicle = + planner_param_.common.attention_area_angle_threshold; + const double dist_margin = planner_param_.common.attention_area_margin; + + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { + continue; + } + const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); + const double pose_angle = tf2::getYaw(pose.orientation); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); + } + } + } + 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, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) +{ + const double intersection_velocity = + planner_param_.collision_detection.velocity_profile.default_velocity; + const double minimum_ego_velocity = + planner_param_.collision_detection.velocity_profile.minimum_default_velocity; + const bool use_upstream_velocity = + planner_param_.collision_detection.velocity_profile.use_upstream; + const double minimum_upstream_velocity = + 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; + + // crop intersection part of the path, and set the reference velocity to intersection_velocity + // for ego's ttc + PathWithLaneId reference_path; + std::optional upstream_stopline{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stopline_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stopline) { + upstream_stopline = i; + } + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } + reference_path.points.push_back(reference_point); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); + if (assigned_lane_found && !has_objective_lane_id) { + break; + } + assigned_lane_found = has_objective_lane_id; + } + if (!assigned_lane_found) { + return {{0.0, 0.0}}; // has already passed the intersection. + } + + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + + // apply smoother to reference velocity + PathWithLaneId smoothed_reference_path = reference_path; + if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { + smoothed_reference_path = reference_path; + } + + // calculate when ego is going to reach each (interpolated) points on the path + TimeDistanceArray time_distance_array{}; + double dist_sum = 0.0; + double passing_time = time_delay; + time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stopline_candidate_idx` makes no sense + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stopline_point, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); + passing_time += (dist / passing_velocity); + + time_distance_array.emplace_back(passing_time, dist_sum); + } + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id_); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); + } + return time_distance_array; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp new file mode 100644 index 0000000000000..353826070eff7 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -0,0 +1,407 @@ +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include +#include // for toPolygon2d + +#include +#include + +#include + +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +std::tuple< + IntersectionModule::OcclusionType, bool /* module detection with margin */, + 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 auto & adjacent_lanelets = intersection_lanelets.adjacent(); + 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 = + (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); + // 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) + : OcclusionType::NOT_OCCLUDED; + occlusion_stop_state_machine_.setStateWithMarginTime( + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection + // distinguish if ego detected occlusion or RTC detects occlusion + const bool ext_occlusion_requested = + (is_occlusion_cleared_with_margin && !occlusion_activated_); // RTC's detection + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } + const bool is_occlusion_state = + (!is_occlusion_cleared_with_margin || ext_occlusion_requested); // including approval + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } + return {occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state}; +} + +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 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; + + const auto & path_ip = interpolated_path_info.path; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + + const auto first_attention_area_idx = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); + if (!first_attention_area_idx) { + return OcclusionType::NOT_OCCLUDED; + } + + const auto first_inside_attention_idx_ip_opt = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); + const std::pair lane_attention_interval_ip = + first_inside_attention_idx_ip_opt + ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) + : lane_interval_ip; + const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; + + const int width = occ_grid.info.width; + const int height = occ_grid.info.height; + const double resolution = occ_grid.info.resolution; + const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; + + Polygon2d grid_poly; + grid_poly.outer().emplace_back(origin.x, origin.y); + grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); + grid_poly.outer().emplace_back( + origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y); + bg::correct(grid_poly); + + auto findCommonCvPolygons = + [&](const auto & area2d, std::vector> & cv_polygons) -> void { + tier4_autoware_utils::Polygon2d area2d_poly; + for (const auto & p : area2d) { + area2d_poly.outer().emplace_back(p.x(), p.y()); + } + area2d_poly.outer().push_back(area2d_poly.outer().front()); + bg::correct(area2d_poly); + std::vector common_areas; + bg::intersection(area2d_poly, grid_poly, common_areas); + if (common_areas.empty()) { + return; + } + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + for (const auto & common_area : common_areas) { + std::vector cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / resolution); + const int idx_y = static_cast((p.y() - origin.y) / resolution); + cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + cv_polygons.push_back(cv_polygon); + } + }; + + // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 + // NOTE: interesting area is set to 255 for later masking + cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); + std::vector> attention_area_cv_polygons; + for (const auto & attention_area : attention_areas) { + const auto area2d = lanelet::utils::to2D(attention_area); + findCommonCvPolygons(area2d, attention_area_cv_polygons); + } + for (const auto & poly : attention_area_cv_polygons) { + cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); + } + // (1.1) + // reset adjacent_lanelets area to 0 on attention_mask + std::vector> adjacent_lane_cv_polygons; + for (const auto & adjacent_lanelet : adjacent_lanelets) { + const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); + findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); + } + for (const auto & poly : adjacent_lane_cv_polygons) { + cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); + } + + // (2) prepare unknown mask + // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] + // unknown: 255 + // not-unknown: 0 + cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + const int idx = y * width + x; + const unsigned char intensity = occ_grid.data.at(idx); + if ( + planner_param_.occlusion.free_space_max <= intensity && + intensity < planner_param_.occlusion.occupied_min) { + unknown_mask_raw.at(height - 1 - y, x) = 255; + } + } + } + // (2.1) apply morphologyEx + const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); + cv::morphologyEx( + unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); + + // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; + cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // 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); + } + 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); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); + } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + } + } + } + + // (4) extract occlusion polygons + const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; + const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; + const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; + const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; + std::vector> contours; + cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + std::vector> valid_contours; + for (const auto & contour : contours) { + if (contour.size() <= 2) { + continue; + } + std::vector approx_contour; + cv::approxPolyDP( + contour, approx_contour, + std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); + if (approx_contour.size() <= 2) continue; + // check area + const double poly_area = cv::contourArea(approx_contour); + if (poly_area < possible_object_area) continue; + // check bounding box size + const auto bbox = cv::minAreaRect(approx_contour); + if (const auto size = bbox.size; std::min(size.height, size.width) < + std::min(possible_object_bbox_x, possible_object_bbox_y) || + std::max(size.height, size.width) < + std::max(possible_object_bbox_x, possible_object_bbox_y)) { + continue; + } + valid_contours.push_back(approx_contour); + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : approx_contour) { + const double glob_x = (p.x + 0.5) * resolution + origin.x; + const double glob_y = (height - 0.5 - p.y) * resolution + origin.y; + point_msg.x = glob_x; + point_msg.y = glob_y; + point_msg.z = origin.z; + polygon_msg.points.push_back(point_msg); + } + debug_data_.occlusion_polygons.push_back(polygon_msg); + } + // (4.1) re-draw occluded cells using valid_contours + occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + for (const auto & valid_contour : valid_contours) { + // NOTE: drawContour does not work well + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); + } + + // (5) find distance + // (5.1) discretize path_ip with resolution for computational cost + LineString2d path_linestring; + path_linestring.emplace_back( + path_ip.points.at(lane_start_idx).point.pose.position.x, + path_ip.points.at(lane_start_idx).point.pose.position.y); + { + auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; + for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { + const auto path_linestring_point = path_ip.points.at(i).point.pose.position; + if ( + tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + 1.0 /* rough tick for computational cost */) { + continue; + } + path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); + prev_path_linestring_point = path_linestring_point; + } + } + + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; + } + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; + struct NearestOcclusionPoint + { + int64 division_index{0}; + int64 point_index{0}; + double dist{0.0}; + geometry_msgs::msg::Point point; + geometry_msgs::msg::Point projection; + } nearest_occlusion_point; + double min_dist = std::numeric_limits::infinity(); + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { + continue; + } + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + division_index, std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + } + } + } + } + + if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { + return OcclusionType::NOT_OCCLUDED; + } + + debug_data_.nearest_occlusion_projection = + std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); + 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); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + return OcclusionType::STATICALLY_OCCLUDED; +} +} // namespace behavior_velocity_planner 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 new file mode 100644 index 0000000000000..746e615d8c6b0 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -0,0 +1,869 @@ +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include // for to_bg2d +#include // for planning_utils:: +#include +#include // for lanelet::autoware::RoadMarking +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + +namespace +{ +namespace bg = boost::geometry; + +lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets previous_lanelets; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return previous_lanelets; + } + previous_lanelets.push_back(ll); + } + return previous_lanelets; +} + +// end inclusive +lanelet::ConstLanelet generatePathLanelet( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const size_t end_idx, const double width, const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + size_t prev_idx = start_idx; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + prev_idx = i; + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + // NOTE: maybe this is opposite + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + +std::optional> getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons, const bool search_forward = true) +{ + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } + } + } + return std::nullopt; +} + +double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +intersection::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_); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const auto & current_pose = planner_data_->current_odometry->pose; + + // spline interpolation + 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"}); + } + + 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_)}); + } + + // 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 + intersection_lanelets.update( + is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); + + const auto & conflicting_lanelets = intersection_lanelets.conflicting(); + const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); + 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"}); + } + 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 + const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value() + : first_conflicting_lane; + + const auto intersection_stoplines_opt = generateIntersectionStopLines( + 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"}); + } + const auto & intersection_stoplines = intersection_stoplines_opt.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); + const auto & conflicting_area = intersection_lanelets.conflicting_area(); + const auto lanelets_on_path = + planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); + // see the doc for struct PathLanelets + const auto path_lanelets_opt = generatePathLanelets( + 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"}); + } + const auto & path_lanelets = path_lanelets_opt.value(); + + if (!occlusion_attention_divisions_) { + occlusion_attention_divisions_ = generateDetectionLaneDivisions( + intersection_lanelets.occlusion_attention(), routing_graph_ptr, + planner_data_->occupancy_grid->info.resolution); + } + + return Result::make_ok( + BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); +} + +std::optional IntersectionModule::getStopLineIndexFromMap( + const intersection::InterpolatedPathInfo & interpolated_path_info, + lanelet::ConstLanelet assigned_lanelet) +{ + const auto & path = interpolated_path_info.path; + const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); + + const auto road_markings = + assigned_lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stopline; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. + } + } + if (stopline.empty()) { + return std::nullopt; + } + + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = + planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); + + for (size_t i = lane_interval.first; i < lane_interval.second; i++) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + std::vector collision_points; + bg::intersection(extended_stopline, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + return i; + } + + geometry_msgs::msg::Pose stop_point_from_map; + stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); + + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); +} + +std::optional +IntersectionModule::generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, + const intersection::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) +{ + const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; + const double stopline_margin = planner_param_.common.default_stopline_margin; + const double max_accel = planner_param_.common.max_accel; + const double max_jerk = planner_param_.common.max_jerk; + const double delay_response_time = planner_param_.common.delay_response_time; + const double peeking_offset = planner_param_.occlusion.peeking_offset; + + const auto first_attention_area = first_attention_lane.polygon3d(); + const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); + const auto & path_ip = interpolated_path_info.path; + const double ds = interpolated_path_info.ds; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); + const int base2front_idx_dist = std::ceil(baselink2front / ds); + + // find the index of the first point whose vehicle footprint on it intersects with attention_area + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const std::optional first_footprint_inside_1st_attention_ip_opt = + util::getFirstPointInsidePolygonByFootprint( + first_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (!first_footprint_inside_1st_attention_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_1st_attention_ip = + first_footprint_inside_1st_attention_ip_opt.value(); + + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // NOTE: maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + + // (1) default stop line position on interpolated path + bool default_stopline_valid = true; + int stop_idx_ip_int = -1; + if (const auto map_stop_idx_ip = + getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); + map_stop_idx_ip) { + stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stopline_valid = false; + } + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + + // (2) ego front stop line position on interpolated path + const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + + // (3) occlusion peeking stop line position on interpolated path + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); + bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the attention area, invalid + { + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = + first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); + } + const auto occlusion_peeking_line_ip = static_cast( + std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + + // (4) first attention stopline position on interpolated path + const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; + const bool first_attention_stopline_valid = true; + + // (5) 1st pass judge line position on interpolated path + const double velocity = planner_data_->current_velocity->twist.linear.x; + const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; + const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int first_pass_judge_ip_int = + static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); + const auto first_pass_judge_line_ip = static_cast( + std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( + 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); + + // (6) stuck vehicle stopline position on interpolated path + 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) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; + } else { + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; + } + } else { + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; + } + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + + // (7) second attention stopline position on interpolated path + int second_attention_stopline_ip_int = -1; + bool second_attention_stopline_valid = false; + if (second_attention_area_opt) { + const auto & second_attention_area = second_attention_area_opt.value(); + std::optional first_footprint_inside_2nd_attention_ip_opt = + util::getFirstPointInsidePolygonByFootprint( + second_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (first_footprint_inside_2nd_attention_ip_opt) { + second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + second_attention_stopline_valid = true; + } + } + const auto second_attention_stopline_ip = + 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; + + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t second_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; + size_t first_pass_judge_line{0}; + size_t second_pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, + {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( + [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { + const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; + const auto insert_idx = util::insertPointIndex( + insert_point, original_path, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + if (!insert_idx) { + return std::nullopt; + } + *stop_idx = insert_idx.value(); + } + if ( + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; + } + + intersection::IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; + } + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; + } + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; + } + if (second_attention_stopline_valid) { + intersection_stoplines.second_attention_stopline = + intersection_stoplines_temp.second_attention_stopline; + } + if (occlusion_peeking_line_valid) { + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; + } + 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; +} + +intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet) +{ + const double detection_area_length = planner_param_.common.attention_area_length; + const double occlusion_detection_area_length = + planner_param_.occlusion.occlusion_attention_area_length; + const bool consider_wrong_direction_vehicle = + planner_param_.collision_detection.consider_wrong_direction_vehicle; + + // retrieve a stopline associated with a traffic light + bool has_traffic_light = false; + if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + + // for low priority lane + // 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(); + for (const auto & right_of_way : right_of_ways) { + if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { + for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { + yield_lanelets.push_back(yield_lanelet); + for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { + yield_lanelets.push_back(previous_lanelet); + } + } + } + } + + // get all following lanes of previous lane + lanelet::ConstLanelets ego_lanelets{}; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + ego_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { + continue; + } + ego_lanelets.push_back(following_lanelet); + } + } + + // get conflicting lanes on assigned lanelet + const auto & conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + std::vector adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } + + // final objective lanelets + lanelet::ConstLanelets detection_lanelets; + lanelet::ConstLanelets conflicting_ex_ego_lanelets; + // conflicting lanes is necessary to get stopline for stuck vehicle + for (auto && conflicting_lanelet : conflicting_lanelets) { + if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) + conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); + } + + // exclude yield lanelets and ego lanelets from detection_lanelets + if (turn_direction_ == std::string("straight") && has_traffic_light) { + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + } else { + if (consider_wrong_direction_vehicle) { + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); + } + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if ( + lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || + lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + } + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + lanelet::ConstLanelets detection_and_preceding_lanelets; + { + const double length = detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } + } + } + } + + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + + intersection::IntersectionLanelets result; + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stopline{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + if (stopline) break; + } + result.attention_stoplines_.push_back(stopline); + } + result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + } + result.attention_non_preceding_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); + result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched + result.attention_area_ = util::getPolygon3dFromLanelets(result.attention_); + result.attention_non_preceding_area_ = + util::getPolygon3dFromLanelets(result.attention_non_preceding_); + result.conflicting_area_ = util::getPolygon3dFromLanelets(result.conflicting_); + result.adjacent_area_ = util::getPolygon3dFromLanelets(result.adjacent_); + result.occlusion_attention_area_ = util::getPolygon3dFromLanelets(result.occlusion_attention_); + return result; +} + +std::optional IntersectionModule::generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx) +{ + const double width = planner_data_->vehicle_info_.vehicle_width_m; + static constexpr double path_lanelet_interval = 1.5; + + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; + + intersection::PathLanelets path_lanelets; + // prev + path_lanelets.prev = ::getPrevLanelets(lanelets_on_path, associative_ids_); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + ::generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? ::getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : ::getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } + return path_lanelets; +} + +std::vector IntersectionModule::generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) +{ + const double curvature_threshold = + planner_param_.occlusion.attention_lane_crop_curvature_threshold; + const double curvature_calculation_ds = + planner_param_.occlusion.attention_lane_curvature_calculation_ds; + + using lanelet::utils::getCenterlineWithOffset; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = ::getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + const auto [merged_detection_lanelets, originals] = + util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); + double area = 0; + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); + } + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); + const double width = area / length; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); + } + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); + } + return detection_divisions; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp new file mode 100644 index 0000000000000..7f23bed3c36cd --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -0,0 +1,380 @@ +// 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 "scene_intersection.hpp" +#include "util.hpp" + +#include // for toGeomPoly +#include +#include + +#include +#include +#include + +#include +#include + +namespace +{ +lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +std::optional IntersectionModule::isStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::IntersectionStopLines & intersection_stoplines, + const intersection::PathLanelets & path_lanelets) 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 bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets); + const auto first_conflicting_lane = + intersection_lanelets.first_conflicting_lane().value(); // this is OK + const bool is_first_conflicting_lane_private = + (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + 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 (stuck_detected) { + if ( + is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane) { + // do nothing + } else { + std::optional stopline_idx = std::nullopt; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = default_stopline_idx_opt.value(); + } else if ( + first_attention_stopline_idx_opt && + fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return intersection::StuckStop{ + closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + } + } + } + return std::nullopt; +} + +bool IntersectionModule::isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + const intersection::PathLanelets & path_lanelets) const +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { + return false; + } + + const auto & objects_ptr = planner_data_->predicted_objects; + + // considering lane change in the intersection, these lanelets are generated from the path + const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; + Polygon2d stuck_vehicle_detect_area{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return false; + } + + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } + const auto target_polygon = + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); + + if (target_polygon.empty()) { + return false; + } + + for (const auto & p : target_polygon) { + stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); + } + + stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); + bg::correct(stuck_vehicle_detect_area); + + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area) { + debug_data_.stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + +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 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 stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + + const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( + target_objects, 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; + const bool is_before_first_attention_stopline = + fromEgoDist(first_attention_stopline_idx) >= 0.0; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (is_before_default_stopline) { + stopline_idx = default_stopline_idx; + } else if (is_before_first_attention_stopline) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + } + } + return std::nullopt; +} + +bool IntersectionModule::checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) const +{ + const bool yield_stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.yield_stuck.turn_direction.straight); + }(); + if (!yield_stuck_detection_direction) { + return false; + } + + const double width = planner_data_->vehicle_info_.vehicle_width_m; + const double stuck_vehicle_vel_thr = + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; + const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; + + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction_ == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction_ == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { + continue; + } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + ::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) { + 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); + + 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); + if (is_in_lanelet) { + debug_data_.yield_stuck_targets.objects.push_back(object.object); + return true; + } + } + } + return false; +} +} // namespace behavior_velocity_planner 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 b929959e43f12..8b0511c741b4e 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 @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -51,7 +52,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -120,7 +121,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR } const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( + const auto first_conflicting_idx_opt = util::getFirstPointInsidePolygonByFootprint( first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, baselink2front); if (!first_conflicting_idx_opt) { return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 6afc2d61d77f2..dfdfb01fb2234 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -14,14 +14,13 @@ #include "util.hpp" -#include "util_type.hpp" +#include "interpolated_path_info.hpp" #include #include #include #include #include -#include #include #include @@ -29,23 +28,22 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include #include -namespace behavior_velocity_planner +namespace behavior_velocity_planner::util { namespace bg = boost::geometry; -namespace util -{ - static std::optional getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) @@ -135,7 +133,8 @@ std::optional> findLaneIdsInterval( } std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -155,6 +154,34 @@ std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -333,12 +360,12 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { - InterpolatedPathInfo interpolated_path_info; + intersection::InterpolatedPathInfo interpolated_path_info; if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger)) { return std::nullopt; } @@ -367,5 +394,14 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -} // namespace util -} // namespace behavior_velocity_planner +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +} // namespace behavior_velocity_planner::util diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 86a34d1e95114..f103191b2dc6f 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,25 +15,22 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "util_type.hpp" +#include "interpolated_path_info.hpp" -#include +#include +#include -#include -#include +#include + +#include +#include -#include -#include #include #include -#include -#include #include #include -namespace behavior_velocity_planner -{ -namespace util +namespace behavior_velocity_planner::util { /** @@ -111,7 +108,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); * @fn * @brief interpolate PathWithLaneId */ -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); @@ -139,10 +136,20 @@ mergeLaneletsByTopologicalSort( * polygon */ std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); -} // namespace util -} // namespace behavior_velocity_planner +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec); + +} // namespace behavior_velocity_planner::util #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index d5e9eeddb6377..c4192750af1d5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -35,8 +35,7 @@ using tier4_autoware_utils::getOrDeclareParameter; NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); auto & pp = planner_param_; diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index e2c29ef868257..9fa5634c6dd65 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -323,6 +323,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; @@ -334,9 +339,12 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - if ( - is_unknown_observation && - planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + old_data->second; + // update timestamp planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = msg->stamp; } else { diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index fcde16d8a871c..d8b042ec880e4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -52,6 +53,7 @@ using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; +using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -251,6 +253,21 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index 5534228c1b86f..196f7c6296cb4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -3,8 +3,8 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data - suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet: - specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet + specify_decel_jerk: true # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles @@ -12,15 +12,7 @@ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - detection_area: - margin_behind: 0.5 # [m] ahead margin for detection area length - margin_ahead: 1.0 # [m] behind margin for detection area length - - # This area uses points not filtered by vector map to ensure safety - mandatory_area: - decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area - - # parameter to create abstracted dynamic obstacles + # Parameter to create abstracted dynamic obstacles dynamic_obstacle: use_mandatory_area: false # [-] whether to use mandatory detection area assume_fixed_velocity: @@ -34,26 +26,42 @@ time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method - # approach if ego has stopped in the front of the obstacle for a certain amount of time - approaching: - enable: false - margin: 0.0 # [m] distance on how close ego approaches the obstacle - limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - - # parameters for the change of state. used only when approaching is enabled - state: - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value - keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition - - # parameter to avoid sudden stopping + # Parameter to prevent sudden stopping. + # If the deceleration jerk and acceleration exceed this value upon inserting a stop point, + # the deceleration will be moderated to stay under this value. slow_down_limit: - enable: true + enable: false max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. - # prevent abrupt stops caused by false positives in perception + # Parameter to prevent abrupt stops caused by false positives in perception ignore_momentary_detection: enable: true time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration + + # Typically used when the "detection_method" is set to ObjectWithoutPath or Points + # Approach if the ego has stopped in front of the obstacle for a certain period + # This avoids the issue of the ego continuously stopping in front of the obstacle + approaching: + enable: false + margin: 0.0 # [m] distance on how close ego approaches the obstacle + limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + # Parameters for state change when "approaching" is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value + keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition + + # Only used when "detection_method" is set to Points + # Filters points by the detection area polygon to reduce computational cost + # The detection area is calculated based on the current velocity and acceleration and deceleration jerk constraints + detection_area: + margin_behind: 0.5 # [m] ahead margin for detection area length + margin_ahead: 1.0 # [m] behind margin for detection area length + + # Only used when "detection_method" is set to Points + # Points in this area are detected even if it is in the no obstacle segmentation area + # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints + mandatory_area: + decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index 809579b383461..606c63ea3448c 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -10,6 +10,7 @@ Tomoya Kimura Shumpei Wakabayashi Takayuki Murooka + Kosuke Takeuchi Apache License 2.0 diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 3ba9bf8bf52e6..f2ef366cbe121 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -105,16 +105,14 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.enable = getOrDeclareParameter(node, ns_a + ".enable"); p.margin = getOrDeclareParameter(node, ns_a + ".margin"); p.limit_vel_kmph = getOrDeclareParameter(node, ns_a + ".limit_vel_kmph"); - } - { - auto & p = planner_param_.state_param; - const std::string ns_s = ns + ".state"; - p.stop_thresh = getOrDeclareParameter(node, ns_s + ".stop_thresh"); - p.stop_time_thresh = getOrDeclareParameter(node, ns_s + ".stop_time_thresh"); - p.disable_approach_dist = getOrDeclareParameter(node, ns_s + ".disable_approach_dist"); - p.keep_approach_duration = - getOrDeclareParameter(node, ns_s + ".keep_approach_duration"); + const std::string ns_as = ns_a + ".state"; + p.state.stop_thresh = getOrDeclareParameter(node, ns_as + ".stop_thresh"); + p.state.stop_time_thresh = getOrDeclareParameter(node, ns_as + ".stop_time_thresh"); + p.state.disable_approach_dist = + getOrDeclareParameter(node, ns_as + ".disable_approach_dist"); + p.state.keep_approach_duration = + getOrDeclareParameter(node, ns_as + ".keep_approach_duration"); } { diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index b14152863d8d0..50b69fc6139c9 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -46,7 +46,7 @@ RunOutModule::RunOutModule( planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), - state_machine_(std::make_unique(planner_param.state_param)) + state_machine_(std::make_unique(planner_param.approaching.state)) { velocity_factor_.init(PlanningBehavior::UNKNOWN); diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 5524c0f76049d..c4976a119dd00 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -85,13 +85,6 @@ struct MandatoryArea float decel_jerk; }; -struct ApproachingParam -{ - bool enable; - float margin; - float limit_vel_kmph; -}; - struct StateParam { float stop_thresh; @@ -100,6 +93,14 @@ struct StateParam float keep_approach_duration; }; +struct ApproachingParam +{ + bool enable; + float margin; + float limit_vel_kmph; + StateParam state; +}; + struct SlowDownLimit { bool enable; @@ -143,7 +144,6 @@ struct PlannerParam DetectionArea detection_area; MandatoryArea mandatory_area; ApproachingParam approaching; - StateParam state_param; DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index fa1a516c107b0..89817f3342dbd 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -31,8 +31,7 @@ using tier4_autoware_utils::getOrDeclareParameter; TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), - getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc")) + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); diff --git a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml index f21e3d12db56f..07f493edcde12 100644 --- a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml +++ b/planning/behavior_velocity_walkway_module/config/walkway.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: walkway: - stop_duration: 1.0 # [s] stop time at stop position + stop_duration: 0.1 # [s] stop time at stop position stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/planning/external_velocity_limit_selector/README.md b/planning/external_velocity_limit_selector/README.md index 2dc76dd3aec6e..92579bfd0abce 100644 --- a/planning/external_velocity_limit_selector/README.md +++ b/planning/external_velocity_limit_selector/README.md @@ -51,7 +51,17 @@ Example: ## Parameters -{{ json_to_markdown("planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json") }} +| Parameter | Type | Description | +| ----------------- | ------ | ------------------------------------------ | +| `max_velocity` | double | default max velocity [m/s] | +| `normal.min_acc` | double | minimum acceleration [m/ss] | +| `normal.max_acc` | double | maximum acceleration [m/ss] | +| `normal.min_jerk` | double | minimum jerk [m/sss] | +| `normal.max_jerk` | double | maximum jerk [m/sss] | +| `limit.min_acc` | double | minimum acceleration to be observed [m/ss] | +| `limit.max_acc` | double | maximum acceleration to be observed [m/ss] | +| `limit.min_jerk` | double | minimum jerk to be observed [m/sss] | +| `limit.max_jerk` | double | maximum jerk to be observed [m/sss] | ## Assumptions / Known limits diff --git a/planning/external_velocity_limit_selector/config/default.param.yaml b/planning/external_velocity_limit_selector/config/default.param.yaml new file mode 100644 index 0000000000000..8023776e191ba --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/external_velocity_limit_selector/config/default_common.param.yaml new file mode 100644 index 0000000000000..a23570a5fc791 --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default_common.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # constraints param for normal driving + normal: + min_acc: -0.5 # min deceleration [m/ss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # constraints to be observed + limit: + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml b/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml deleted file mode 100644 index 472b9370607b5..0000000000000 --- a/planning/external_velocity_limit_selector/config/external_velocity_limit_selector.param.yaml +++ /dev/null @@ -1,18 +0,0 @@ -/**: - ros__parameters: - # constraints param for normal driving - normal: - min_acc: -0.5 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.5 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] - - # constraints to be observed - limit: - min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] - - # motion state constraints - max_velocity: 20.0 # max velocity limit [m/s] diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index c499c0741f09f..5ef089f3d3ee7 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -1,6 +1,7 @@ - + + @@ -10,7 +11,8 @@ - + + diff --git a/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json b/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json deleted file mode 100644 index 36fdaf31cbc6f..0000000000000 --- a/planning/external_velocity_limit_selector/schema/external_velocity_limit_selector.schema.json +++ /dev/null @@ -1,85 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for external velocity limit selector node", - "type": "object", - "definitions": { - "external_velocity_limit_selector": { - "type": "object", - "properties": { - "max_velocity": { - "type": "number", - "description": "max velocity limit [m/s]", - "default": 20.0 - }, - "normal": { - "type": "object", - "properties": { - "min_acc": { - "type": "number", - "description": "min deceleration [m/ss]", - "default": -0.5 - }, - "max_acc": { - "type": "number", - "description": "max acceleration [m/ss]", - "default": 1.0 - }, - "min_jerk": { - "type": "number", - "description": "min jerk [m/sss]", - "default": -0.5 - }, - "max_jerk": { - "type": "number", - "description": "max jerk [m/sss]", - "default": 1.0 - } - }, - "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] - }, - "limit": { - "type": "object", - "properties": { - "min_acc": { - "type": "number", - "description": "min deceleration to be observed [m/ss]", - "default": -2.5 - }, - "max_acc": { - "type": "number", - "description": "max acceleration to be observed [m/ss]", - "default": 1.0 - }, - "min_jerk": { - "type": "number", - "description": "min jerk to be observed [m/sss]", - "default": -1.5 - }, - "max_jerk": { - "type": "number", - "description": "max jerk to be observed [m/sss]", - "default": 1.5 - } - }, - "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] - } - }, - "required": ["max_velocity", "normal", "limit"], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/external_velocity_limit_selector" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 5d1c0c68d8208..e98084204ba78 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -292,7 +292,8 @@ bool DefaultPlanner::check_goal_footprint( lanelet::ConstLanelets lanelets; lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); - lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets); + lanelet::ConstLanelet combined_lanelets = + combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_); // if next lanelet length longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -351,7 +352,8 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets - lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets); + lanelet::ConstLanelet combined_prev_lanelet = + combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_); // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 690a864fcdacd..e3983b7f3b962 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -54,7 +54,8 @@ void insert_marker_array( a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets) { lanelet::Points3d lefts; lanelet::Points3d rights; @@ -70,17 +71,48 @@ lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) } for (const auto & llt : lanelets) { - if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { - for (const auto & pt : llt.leftBound()) { - lefts.push_back(lanelet::Point3d(pt)); - } + // lambda to check if shoulder lane which share left bound with lanelets exist + const auto find_bound_shared_shoulder = + [&shoulder_lanelets](const auto & lanelet_bound, const auto & get_shoulder_bound) { + return std::find_if( + shoulder_lanelets.begin(), shoulder_lanelets.end(), + [&lanelet_bound, &get_shoulder_bound](const auto & shoulder_llt) { + return lanelet_bound.id() == get_shoulder_bound(shoulder_llt).id(); + }); + }; + + // lambda to add bound to target_bound + const auto add_bound = [](const auto & bound, auto & target_bound) { + std::transform( + bound.begin(), bound.end(), std::back_inserter(target_bound), + [](const auto & pt) { return lanelet::Point3d(pt); }); + }; + + // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist + const auto left_shared_shoulder_it = find_bound_shared_shoulder( + llt.leftBound(), [](const auto & shoulder_llt) { return shoulder_llt.rightBound(); }); + if (left_shared_shoulder_it != shoulder_lanelets.end()) { + // if exist, add left bound of SHOULDER lanelet to lefts + add_bound(left_shared_shoulder_it->leftBound(), lefts); + } else if ( + // if not exist, add left bound of lanelet to lefts + std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { + add_bound(llt.leftBound(), lefts); } - if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { - for (const auto & pt : llt.rightBound()) { - rights.push_back(lanelet::Point3d(pt)); - } + + // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist + const auto right_shared_shoulder_it = find_bound_shared_shoulder( + llt.rightBound(), [](const auto & shoulder_llt) { return shoulder_llt.leftBound(); }); + if (right_shared_shoulder_it != shoulder_lanelets.end()) { + // if exist, add right bound of SHOULDER lanelet to rights + add_bound(right_shared_shoulder_it->rightBound(), rights); + } else if ( + // if not exist, add right bound of lanelet to rights + std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { + add_bound(llt.rightBound(), rights); } } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 3ea6237f38501..3287267a00dfe 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -49,7 +49,8 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); -lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets); +lanelet::ConstLanelet combine_lanelets_with_shoulder( + const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 9ee874928d7d1..e0181065150fd 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -731,6 +731,7 @@ bool MissionPlanner::check_reroute_safety( const LaneletRoute & original_route, const LaneletRoute & target_route) { if (original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || !odometry_) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Route, map or odometry is not set."); return false; } @@ -803,6 +804,8 @@ bool MissionPlanner::check_reroute_safety( return std::nullopt; }); if (!start_idx_opt.has_value()) { + RCLCPP_ERROR( + get_logger(), "Check reroute safety failed. Cannot find the start index of the route."); return false; } const size_t start_idx = start_idx_opt.value(); @@ -838,6 +841,7 @@ bool MissionPlanner::check_reroute_safety( // get closest lanelet in start lanelets lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet."); return false; } diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 90f7295c4cf6f..5138da6a43c0c 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -453,6 +453,9 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar return; } + // Set 0 at the end of the trajectory + input_points.back().longitudinal_velocity_mps = 0.0; + // calculate prev closest point if (!prev_output_.empty()) { current_closest_point_from_prev_output_ = calcProjectedTrajectoryPointFromEgo(prev_output_); 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 d8b9c942a986a..aad4bd7e17757 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -85,15 +85,15 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.2 # lateral margin between obstacle and trajectory band with ego's width crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: - obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + 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 slow_down: @@ -188,7 +188,7 @@ static: min_lat_margin: 0.2 max_lat_margin: 1.0 - min_ego_velocity: 2.0 + min_ego_velocity: 4.0 max_ego_velocity: 8.0 pedestrian: moving: diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index f00ad01efb0d9..f7ce218cf3ccf 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -37,19 +37,21 @@ StopSpeedExceeded createStopSpeedExceededMsg( } tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( - const rclcpp::Time & current_time, const geometry_msgs::msg::Pose & stop_pose, + const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose, const StopObstacle & stop_obstacle) { // create header std_msgs::msg::Header header; header.frame_id = "map"; - header.stamp = current_time; + header.stamp = planner_data.current_time; // create stop factor StopFactor stop_factor; stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; stop_factor_point.z = stop_pose.position.z; + stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( + planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); stop_factor.stop_factor_points.emplace_back(stop_factor_point); // create stop reason stamped @@ -359,7 +361,7 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = - makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); + makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 037c7fb0f4e95..a12eb7dedcf88 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -64,6 +64,7 @@ class PlanningValidator : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg); + bool checkValidSize(const Trajectory & trajectory) const; bool checkValidFiniteValue(const Trajectory & trajectory); bool checkValidInterval(const Trajectory & trajectory); bool checkValidRelativeAngle(const Trajectory & trajectory); @@ -116,7 +117,7 @@ class PlanningValidator : public rclcpp::Node vehicle_info_util::VehicleInfo vehicle_info_; - bool isAllValid(const PlanningValidatorStatus & status); + bool isAllValid(const PlanningValidatorStatus & status) const; Trajectory::ConstSharedPtr current_trajectory_; Trajectory::ConstSharedPtr previous_published_trajectory_; diff --git a/planning/planning_validator/msg/PlanningValidatorStatus.msg b/planning/planning_validator/msg/PlanningValidatorStatus.msg index cfc991b35ffc7..b218e7996888a 100644 --- a/planning/planning_validator/msg/PlanningValidatorStatus.msg +++ b/planning/planning_validator/msg/PlanningValidatorStatus.msg @@ -1,6 +1,7 @@ builtin_interfaces/Time stamp # states +bool is_valid_size bool is_valid_finite_value bool is_valid_interval bool is_valid_relative_angle diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 58af2c08ccb22..6185163811614 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -113,6 +113,9 @@ void PlanningValidator::setupDiag() d->setHardwareID("planning_validator"); std::string ns = "trajectory_validation_"; + d->add(ns + "size", [&](auto & stat) { + setStatus(stat, validation_status_.is_valid_size, "invalid trajectory size is found"); + }); d->add(ns + "finite", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found"); }); @@ -251,14 +254,26 @@ void PlanningValidator::publishDebugInfo() void PlanningValidator::validate(const Trajectory & trajectory) { - if (trajectory.points.size() < 2) { - RCLCPP_ERROR(get_logger(), "trajectory size is less than 2. Cannot validate."); - return; - } - auto & s = validation_status_; + const auto terminateValidation = [&](const auto & ss) { + RCLCPP_ERROR_STREAM(get_logger(), ss); + s.invalid_count += 1; + }; + + s.is_valid_size = checkValidSize(trajectory); + if (!s.is_valid_size) { + return terminateValidation( + "trajectory has invalid point size (" + std::to_string(trajectory.points.size()) + + "). Stop validation process, raise an error."); + } + s.is_valid_finite_value = checkValidFiniteValue(trajectory); + if (!s.is_valid_finite_value) { + return terminateValidation( + "trajectory has invalid value (NaN, Inf, etc). Stop validation process, raise an error."); + } + s.is_valid_interval = checkValidInterval(trajectory); s.is_valid_lateral_acc = checkValidLateralAcceleration(trajectory); s.is_valid_longitudinal_max_acc = checkValidMaxLongitudinalAcceleration(trajectory); @@ -279,6 +294,11 @@ void PlanningValidator::validate(const Trajectory & trajectory) s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; } +bool PlanningValidator::checkValidSize(const Trajectory & trajectory) const +{ + return trajectory.points.size() >= 2; +} + bool PlanningValidator::checkValidFiniteValue(const Trajectory & trajectory) { for (const auto & p : trajectory.points) { @@ -427,12 +447,13 @@ bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajector return true; } -bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) +bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const { - return s.is_valid_finite_value && s.is_valid_interval && s.is_valid_relative_angle && - s.is_valid_curvature && s.is_valid_lateral_acc && s.is_valid_longitudinal_max_acc && - s.is_valid_longitudinal_min_acc && s.is_valid_steering && s.is_valid_steering_rate && - s.is_valid_velocity_deviation && s.is_valid_distance_deviation; + return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval && + s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc && + s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc && + s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation && + s.is_valid_distance_deviation; } void PlanningValidator::displayStatus() @@ -447,6 +468,7 @@ void PlanningValidator::displayStatus() const auto & s = validation_status_; + warn(s.is_valid_size, "planning trajectory size is invalid, too small."); warn(s.is_valid_curvature, "planning trajectory curvature is too large!!"); warn(s.is_valid_distance_deviation, "planning trajectory is too far from ego!!"); warn(s.is_valid_finite_value, "planning trajectory has invalid value!!"); diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml index f8b96ca0db146..37a9abc47bfeb 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml +++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml @@ -21,6 +21,10 @@ name="behavior_path_planner_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml" /> + + diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 51bd9e87d6ba2..17191868b7418 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -19,6 +19,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs behavior_path_planner + behavior_velocity_planner geometry_msgs global_parameter_loader interpolation diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index f2d9dc68a8cdc..d98341ecb2e23 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -425,6 +425,10 @@ std::vector StaticCenterlineOptimizerNode::plan_path( const double behavior_path_interval = has_parameter("output_path_interval") ? get_parameter("output_path_interval").as_double() : declare_parameter("output_path_interval"); + const double behavior_vel_interval = + has_parameter("behavior_output_path_interval") + ? get_parameter("behavior_output_path_interval").as_double() + : declare_parameter("behavior_output_path_interval"); // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { @@ -439,8 +443,7 @@ std::vector StaticCenterlineOptimizerNode::plan_path( // convert path with lane id to path const auto raw_path = [&]() { const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - // NOTE: The behavior_velocity_planner resamples with the interval 1.0 somewhere. - return motion_utils::resamplePath(non_resampled_path, 1.0); + return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(get_logger(), "Converted to path and published."); diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py index 141743deb007c..36bdfc732ed79 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -56,6 +56,10 @@ def generate_test_description(): get_package_share_directory("behavior_path_planner"), "config/behavior_path_planner.param.yaml", ), + os.path.join( + get_package_share_directory("behavior_velocity_planner"), + "config/behavior_velocity_planner.param.yaml", + ), os.path.join( get_package_share_directory("path_smoother"), "config/elastic_band_smoother.param.yaml", diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 1ec7dc1f516ad..6799a1f79ae89 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -4,6 +4,11 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance. +This node subscribes to NavSatFix to publish the pose of **base_link**. The data in NavSatFix represents the antenna's position. Therefore, it performs a coordinate transformation using the tf from `base_link` to the antenna's position. The frame_id of the antenna's position refers to NavSatFix's `header.frame_id`. +(**Note that `header.frame_id` in NavSatFix indicates the antenna's frame_id, not the Earth or reference ellipsoid.** [See also NavSatFix definition.](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html)) + +If the transformation from `base_link` to the antenna cannot be obtained, it outputs the pose of the antenna position without performing coordinate transformation. + ## Inner-workings / Algorithms ## Inputs / Outputs diff --git a/sensing/gnss_poser/config/gnss_poser.param.yaml b/sensing/gnss_poser/config/gnss_poser.param.yaml index 30be98bba8cf1..1c00b43c2066a 100644 --- a/sensing/gnss_poser/config/gnss_poser.param.yaml +++ b/sensing/gnss_poser/config/gnss_poser.param.yaml @@ -2,7 +2,6 @@ ros__parameters: base_frame: base_link gnss_base_frame: gnss_base_link - gnss_frame: gnss map_frame: map buff_epoch: 1 use_gnss_ins_orientation: true diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 2118d33bc4b30..97ca1d8bffe77 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -89,10 +89,9 @@ class GNSSPoser : public rclcpp::Node rclcpp::Publisher::SharedPtr fixed_pub_; MapProjectorInfo::Message projector_info_; - std::string base_frame_; - std::string gnss_frame_; - std::string gnss_base_frame_; - std::string map_frame_; + const std::string base_frame_; + const std::string gnss_base_frame_; + const std::string map_frame_; bool received_map_projector_info_ = false; bool use_gnss_ins_orientation_; diff --git a/sensing/gnss_poser/schema/gnss_poser.schema.json b/sensing/gnss_poser/schema/gnss_poser.schema.json index 1d63bb2f4850e..6d9bc716e121a 100644 --- a/sensing/gnss_poser/schema/gnss_poser.schema.json +++ b/sensing/gnss_poser/schema/gnss_poser.schema.json @@ -11,11 +11,6 @@ "default": "base_link", "description": "frame id for base_frame" }, - "gnss_frame": { - "type": "string", - "default": "gnss", - "description": "frame id for gnss_frame" - }, "gnss_base_frame": { "type": "string", "default": "gnss_base_link", diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 3a18dca815f12..ec58226273441 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -31,7 +31,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) tf2_listener_(tf2_buffer_), tf2_broadcaster_(*this), base_frame_(declare_parameter("base_frame")), - gnss_frame_(declare_parameter("gnss_frame")), gnss_base_frame_(declare_parameter("gnss_base_frame")), map_frame_(declare_parameter("map_frame")), use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), @@ -142,8 +141,9 @@ void GNSSPoser::callbackNavSatFix( // get TF from gnss_antenna to base_link auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); + const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; getStaticTransform( - base_frame_, gnss_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + base_frame_, gnss_frame, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); 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 d096130e65f05..4c3a3aed24d90 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 @@ -280,9 +280,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node * @brief get z-position from trajectory * @param [in] x current x-position * @param [in] y current y-position + * @param [in] prev_odometry odometry calculated in the previous step * @return get z-position from trajectory */ - double get_z_pose_from_trajectory(const double x, const double y); + double get_z_pose_from_trajectory(const double x, const double y, const Odometry & prev_odometry); /** * @brief get transform from two frame_ids 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 8de5be9d71503..37ec5b33014a7 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 @@ -371,9 +371,10 @@ void SimplePlanningSimulator::on_timer() } // set current state + const auto prev_odometry = current_odometry_; current_odometry_ = to_odometry(vehicle_model_ptr_, ego_pitch_angle); current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( - current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); + current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y, prev_odometry); current_velocity_ = to_velocity_report(vehicle_model_ptr_); current_steer_ = to_steering_report(vehicle_model_ptr_); @@ -429,6 +430,7 @@ void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::Co set_initial_state_with_transform(initial_pose, initial_twist); initial_pose_ = msg; + current_odometry_.pose = msg->pose; } void SimplePlanningSimulator::on_initialtwist(const TwistStamped::ConstSharedPtr msg) @@ -591,11 +593,12 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & is_initialized_ = true; } -double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const double y) +double SimplePlanningSimulator::get_z_pose_from_trajectory( + const double x, const double y, const Odometry & prev_odometry) { // calculate closest point on trajectory if (!current_trajectory_ptr_) { - return 0.0; + return prev_odometry.pose.pose.position.z; } const double max_sqrt_dist = std::numeric_limits::max(); @@ -616,7 +619,7 @@ double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const return current_trajectory_ptr_->points.at(index).pose.position.z; } - return 0.0; + return prev_odometry.pose.pose.position.z; } TransformStamped SimplePlanningSimulator::get_transform_msg( diff --git a/system/component_state_monitor/package.xml b/system/component_state_monitor/package.xml index 60d0fd0577ef0..16903fa216174 100644 --- a/system/component_state_monitor/package.xml +++ b/system/component_state_monitor/package.xml @@ -5,8 +5,6 @@ 0.1.0 The component_state_monitor package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 6a9a22f39cdc4..4e691f7ab3192 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -5,8 +5,6 @@ 0.1.0 The default_ad_api package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index e54a5faa6e1b7..785ff58db1f81 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -5,8 +5,6 @@ 0.1.0 The ad_api_adaptors package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/ad_api_visualizers/package.xml b/system/default_ad_api_helpers/ad_api_visualizers/package.xml index 95961769813cd..75fd09a1335b1 100644 --- a/system/default_ad_api_helpers/ad_api_visualizers/package.xml +++ b/system/default_ad_api_helpers/ad_api_visualizers/package.xml @@ -5,8 +5,6 @@ 0.1.0 The ad_api_visualizers package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml index 10f4abfe446fb..be192c3dade4b 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -5,8 +5,6 @@ 0.1.0 The automatic_pose_initializer package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml index 7cdd949ae3cfe..aad8a4ffffac4 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml @@ -22,6 +22,12 @@ type: diagnostic_aggregator/AnalyzerGroup path: trajectory_validation analyzers: + trajectory_size: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_size + contains: [": trajectory_validation_size"] + timeout: 1.0 + trajectory_finite_value: type: diagnostic_aggregator/GenericAnalyzer path: trajectory_validation_finite diff --git a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml index d63a115304a87..1001696d96679 100644 --- a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml +++ b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - csv_path_accel_map: "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv" - csv_path_brake_map: "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv" - csv_path_steer_map: "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv" convert_accel_cmd: true convert_brake_cmd: true convert_steer_cmd: true diff --git a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml index 0a9962a7c2a30..a74cdcc56132d 100644 --- a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml +++ b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -1,5 +1,9 @@ + + + + @@ -9,6 +13,9 @@ + + + diff --git a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index 4b562f401e09b..aa8936d331a87 100644 --- a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -6,21 +6,6 @@ "raw_vehicle_cmd_converter": { "type": "object", "properties": { - "csv_path_accel_map": { - "type": "string", - "description": "path for acceleration map csv file", - "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv" - }, - "csv_path_brake_map": { - "type": "string", - "description": "path for brake map csv file", - "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv" - }, - "csv_path_steer_map": { - "type": "string", - "description": "path for steer map csv file", - "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv" - }, "convert_accel_cmd": { "type": "boolean", "description": "use accel or not", diff --git a/vehicle/vehicle_info_util/Readme.md b/vehicle/vehicle_info_util/Readme.md index c3d06a3b43260..600fd62270d81 100644 --- a/vehicle/vehicle_info_util/Readme.md +++ b/vehicle/vehicle_info_util/Readme.md @@ -7,6 +7,35 @@ This package is to get vehicle info parameters. ### Description In [here](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/), you can check the vehicle dimensions with more detail. +The current format supports only the Ackermann model. This file defines the model assumed in autoware path planning, control, etc. and does not represent the exact physical model. If a model other than the Ackermann model is used, it is assumed that a vehicle interface will be designed to change the control output for the model. + +### Versioning Policy + +We have implemented a versioning system for the `vehicle_info.param.yaml` file to ensure clarity and consistency in file format across different versions of Autoware and its external applications. Please see [discussion](https://github.com/orgs/autowarefoundation/discussions/4050) for the details. + +#### How to Operate + +- The current file format is set as an unversioned base version (`version:` field is commented out). +- For the next update involving changes (such as additions, deletions, or modifications): + - Uncomment and update the version line at the beginning of the file. + - Initiate versioning by assigning a version number, starting from `0.1.0`. Follow the semantic versioning format (MAJOR.MINOR.PATCH). + - Update this Readme.md too. +- For subsequent updates, continue incrementing the version number in accordance with the changes made. + - Discuss how to increment version depending on the amount of changes made to the file. + +```yaml +/**: + ros__parameters: + # version: 0.1.0 # Uncomment and update this line for future format changes. + wheel_radius: 0.383 + ... +``` + +#### Why Versioning? + +- Consistency Across Updates: Implementing version control will allow accurate tracking of changes over time and changes in vehicle information parameters. +- Clarity for External Applications: External applications that depend on `vehicle_info.param.yaml` need to reference the correct file version for optimal compatibility and functionality. +- Simplified Management for Customized Branches: Assigning versions directly to the `vehicle_info.param.yaml` file simplifies management compared to maintaining separate versions for multiple customized Autoware branches. This approach streamlines version tracking and reduces complexity. ### Scripts diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml index 8941b92b4e78e..72c070c17b52c 100644 --- a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml +++ b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + # version: 0.1.0 # uncomment this line in the next update of this file format. please check Readme.md wheel_radius: 0.39 wheel_width: 0.42 wheel_base: 2.74 # between front wheel center and rear wheel center