diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ea86022e7134d..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,39 +76,39 @@ 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_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@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 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 masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp -map/map_projection_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_tf_generator/** azumi.suzuki@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 @@ -122,7 +122,7 @@ perception/euclidean_cluster/** yukihiro.saito@tier4.jp perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/heatmap_visualizer/** kotaro.uetake@tier4.jp -perception/image_projection_based_fusion/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/image_projection_based_fusion/** dai.nguyen@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp @@ -154,13 +154,20 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp 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 zulfaqar.azmi@tier4.jp -planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp -planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@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 tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_planner_common/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_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 +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_start_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_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 satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@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 @@ -168,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 @@ -183,7 +190,7 @@ planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohs planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp +planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp @@ -218,18 +225,18 @@ 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 system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp -system/system_diagnostic_graph/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index f9c29b81b6e6c..1da4d24966de9 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.11.0 + uses: styfle/cancel-workflow-action@0.12.0 with: workflow_id: all all_but_latest: true diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index 95ebb8725f62b..b426d0cba6614 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -30,7 +30,7 @@ jobs: echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 ref: ${{ steps.set-tag-name.outputs.ref-name }} diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index e754ecab24f85..38738196a0bd3 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index b231dbda87938..33c00ee1066ae 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -16,7 +16,7 @@ jobs: private_key: ${{ secrets.PRIVATE_KEY }} - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: ref: ${{ github.event.pull_request.head.ref }} diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index eb18ccdba38d0..1fbf2ff46925c 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Run spell-check uses: autowarefoundation/autoware-github-actions/spell-check@v1 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_common/design/comparisons.md b/common/autoware_auto_common/design/comparisons.md index f3a7eb5e58429..3d00f77c1b0c3 100644 --- a/common/autoware_auto_common/design/comparisons.md +++ b/common/autoware_auto_common/design/comparisons.md @@ -22,8 +22,8 @@ The `exclusive_or` function will test whether two values cast to different boole ## Example Usage ```c++ -#include "common/bool_comparisons.hpp" -#include "common/float_comparisons.hpp" +#include "autoware_auto_common/common/bool_comparisons.hpp" +#include "autoware_auto_common/common/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/include/common/type_traits.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp similarity index 96% rename from common/autoware_auto_common/include/common/type_traits.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp index 7087ed1e81181..66f382f081b33 100644 --- a/common/autoware_auto_common/include/common/type_traits.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp @@ -14,15 +14,15 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" #include #include #include -#ifndef COMMON__TYPE_TRAITS_HPP_ -#define COMMON__TYPE_TRAITS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ namespace autoware { @@ -219,4 +219,4 @@ struct intersect } // namespace common } // namespace autoware -#endif // COMMON__TYPE_TRAITS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_ diff --git a/common/autoware_auto_common/include/common/types.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp similarity index 93% rename from common/autoware_auto_common/include/common/types.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/types.hpp index 3f3e444c1aa8c..1c7dfe48c7ec8 100644 --- a/common/autoware_auto_common/include/common/types.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp @@ -16,11 +16,11 @@ /// \file /// \brief This file includes common type definition -#ifndef COMMON__TYPES_HPP_ -#define COMMON__TYPES_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ -#include "common/visibility_control.hpp" -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/common/visibility_control.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include #include @@ -122,4 +122,4 @@ using void_t = void; } // namespace common } // namespace autoware -#endif // COMMON__TYPES_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_ diff --git a/common/autoware_auto_common/include/common/visibility_control.hpp b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp similarity index 88% rename from common/autoware_auto_common/include/common/visibility_control.hpp rename to common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp index 0a988d6407dfa..33834fd9ccfed 100644 --- a/common/autoware_auto_common/include/common/visibility_control.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef COMMON__VISIBILITY_CONTROL_HPP_ -#define COMMON__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ #if defined(_MSC_VER) && defined(_WIN64) #if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS) @@ -35,4 +35,4 @@ #error "Unsupported Build Configuration" #endif // _MSC_VER -#endif // COMMON__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp similarity index 89% rename from common/autoware_auto_common/include/helper_functions/angle_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp index 6cca2440d5680..ea974774dd9d5 100644 --- a/common/autoware_auto_common/include/helper_functions/angle_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ -#define HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ #include #include @@ -63,4 +63,4 @@ constexpr T wrap_angle(T angle) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp similarity index 85% rename from common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp index c6bf09365af4f..45da098ad0066 100644 --- a/common/autoware_auto_common/include/helper_functions/bool_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp @@ -18,10 +18,10 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ -#include "common/types.hpp" +#include "autoware_auto_common/common/types.hpp" namespace autoware { @@ -47,4 +47,4 @@ types::bool8_t exclusive_or(const T & a, const T & b) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp similarity index 90% rename from common/autoware_auto_common/include/helper_functions/byte_reader.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp index d9e55c4ecfbfe..3852361caebeb 100644 --- a/common/autoware_auto_common/include/helper_functions/byte_reader.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__BYTE_READER_HPP_ -#define HELPER_FUNCTIONS__BYTE_READER_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ #include #include @@ -70,4 +70,4 @@ class ByteReader } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__BYTE_READER_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/crtp.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp similarity index 88% rename from common/autoware_auto_common/include/helper_functions/crtp.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp index 0e8110a9a3bb9..e75674eb73c70 100644 --- a/common/autoware_auto_common/include/helper_functions/crtp.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__CRTP_HPP_ -#define HELPER_FUNCTIONS__CRTP_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ namespace autoware { @@ -49,4 +49,4 @@ class crtp } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__CRTP_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp similarity index 95% rename from common/autoware_auto_common/include/helper_functions/float_comparisons.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp index de1f459f21d0a..1a64fe71a1720 100644 --- a/common/autoware_auto_common/include/helper_functions/float_comparisons.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp @@ -18,8 +18,8 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ -#define HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ #include #include @@ -146,4 +146,4 @@ bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps) } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp similarity index 92% rename from common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp index fb9bdccf32b25..70c29eaf220d8 100644 --- a/common/autoware_auto_common/include/helper_functions/mahalanobis_distance.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ -#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ #include @@ -69,4 +69,4 @@ types::float32_t calculate_mahalanobis_distance( } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp similarity index 94% rename from common/autoware_auto_common/include/helper_functions/message_adapters.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp index 352ef7c7b65d5..d3bda57b3374f 100644 --- a/common/autoware_auto_common/include/helper_functions/message_adapters.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp @@ -16,8 +16,8 @@ /// \file /// \brief This file includes common helper functions -#ifndef HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ -#define HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ #include @@ -112,4 +112,4 @@ TimeStamp get_stamp(const T & msg) noexcept } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/template_utils.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp similarity index 91% rename from common/autoware_auto_common/include/helper_functions/template_utils.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp index b39931a3fd15a..0360908509618 100644 --- a/common/autoware_auto_common/include/helper_functions/template_utils.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#define HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ -#include +#include "autoware_auto_common/common/types.hpp" #include @@ -72,4 +72,4 @@ struct expression_valid_with_return< } // namespace helper_functions } // namespace common } // namespace autoware -#endif // HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_ diff --git a/common/autoware_auto_common/include/helper_functions/type_name.hpp b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp similarity index 84% rename from common/autoware_auto_common/include/helper_functions/type_name.hpp rename to common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp index a95834373d552..106cede1f2f5a 100644 --- a/common/autoware_auto_common/include/helper_functions/type_name.hpp +++ b/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp @@ -14,10 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#define HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ -#include +#include "autoware_auto_common/common/visibility_control.hpp" #include #include @@ -53,4 +53,4 @@ COMMON_PUBLIC std::string get_type_name(const T &) } // namespace helper_functions } // namespace autoware -#endif // HELPER_FUNCTIONS__TYPE_NAME_HPP_ +#endif // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_ diff --git a/common/autoware_auto_common/test/test_angle_utils.cpp b/common/autoware_auto_common/test/test_angle_utils.cpp index 031ae144139aa..810c302845daf 100644 --- a/common/autoware_auto_common/test/test_angle_utils.cpp +++ b/common/autoware_auto_common/test/test_angle_utils.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/angle_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_bool_comparisons.cpp b/common/autoware_auto_common/test/test_bool_comparisons.cpp index 67c1c8ddbf9aa..a84d65e569692 100644 --- a/common/autoware_auto_common/test/test_bool_comparisons.cpp +++ b/common/autoware_auto_common/test/test_bool_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/bool_comparisons.hpp" +#include "autoware_auto_common/helper_functions/bool_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_byte_reader.cpp b/common/autoware_auto_common/test/test_byte_reader.cpp index c83d06c6e8132..a5ab8743f7ed4 100644 --- a/common/autoware_auto_common/test/test_byte_reader.cpp +++ b/common/autoware_auto_common/test/test_byte_reader.cpp @@ -14,9 +14,10 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "common/types.hpp" -#include "gtest/gtest.h" -#include "helper_functions/byte_reader.hpp" +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/byte_reader.hpp" + +#include #include diff --git a/common/autoware_auto_common/test/test_float_comparisons.cpp b/common/autoware_auto_common/test/test_float_comparisons.cpp index d292dc0a0cb20..37d3afdc177d5 100644 --- a/common/autoware_auto_common/test/test_float_comparisons.cpp +++ b/common/autoware_auto_common/test/test_float_comparisons.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "helper_functions/float_comparisons.hpp" +#include "autoware_auto_common/helper_functions/float_comparisons.hpp" #include diff --git a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp index 262599180abb6..2015a85bc2bc8 100644 --- a/common/autoware_auto_common/test/test_mahalanobis_distance.cpp +++ b/common/autoware_auto_common/test/test_mahalanobis_distance.cpp @@ -13,8 +13,8 @@ // limitations under the License. // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/mahalanobis_distance.hpp" #include diff --git a/common/autoware_auto_common/test/test_message_field_adapters.cpp b/common/autoware_auto_common/test/test_message_field_adapters.cpp index 34974c1cda9a6..c35f0ff826995 100644 --- a/common/autoware_auto_common/test/test_message_field_adapters.cpp +++ b/common/autoware_auto_common/test/test_message_field_adapters.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/message_adapters.hpp" #include diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp index 9c9ca9ae4b5f2..a670aaab83cfa 100644 --- a/common/autoware_auto_common/test/test_template_utils.cpp +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_common/helper_functions/template_utils.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_name.cpp b/common/autoware_auto_common/test/test_type_name.cpp index 1fb60d838f307..4ada59b4fb2e1 100644 --- a/common/autoware_auto_common/test/test_type_name.cpp +++ b/common/autoware_auto_common/test/test_type_name.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/types.hpp" +#include "autoware_auto_common/helper_functions/type_name.hpp" #include diff --git a/common/autoware_auto_common/test/test_type_traits.cpp b/common/autoware_auto_common/test/test_type_traits.cpp index 7203ab8d649ee..92d01b3d84d51 100644 --- a/common/autoware_auto_common/test/test_type_traits.cpp +++ b/common/autoware_auto_common/test/test_type_traits.cpp @@ -14,8 +14,8 @@ // // Developed by Apex.AI, Inc. -#include -#include +#include "autoware_auto_common/common/type_traits.hpp" +#include "autoware_auto_common/common/types.hpp" #include diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index 454e0e7ef044f..ee819b7cd797c 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -5,9 +5,9 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED - include/geometry/spatial_hash.hpp - include/geometry/intersection.hpp - include/geometry/spatial_hash_config.hpp + include/autoware_auto_geometry/spatial_hash.hpp + include/autoware_auto_geometry/intersection.hpp + include/autoware_auto_geometry/spatial_hash_config.hpp src/spatial_hash.cpp src/bounding_box.cpp ) diff --git a/common/autoware_auto_geometry/design/interval.md b/common/autoware_auto_geometry/design/interval.md index 26260ba8d8e67..4fe65ff8e0310 100644 --- a/common/autoware_auto_geometry/design/interval.md +++ b/common/autoware_auto_geometry/design/interval.md @@ -39,7 +39,7 @@ See 'Example Usage' below. ## Example Usage ```c++ -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" #include diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp similarity index 95% rename from common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp index d1dee63f73b56..0f3a68e14d442 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/bounding_box_common.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/bounding_box_common.hpp @@ -17,11 +17,11 @@ /// \file /// \brief Common functionality for bounding box computation algorithms -#ifndef GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#define GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" #include #include @@ -185,4 +185,4 @@ std::vector GEOMETRY_PUBLIC get_transformed_corners } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__BOUNDING_BOX_COMMON_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp index f050628f32f25..e0f2e66e87ee5 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/eigenbox_2d.hpp @@ -19,10 +19,10 @@ /// bounding box // cspell: ignore eigenbox, EIGENBOX -#ifndef GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#define GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ -#include +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" #include #include @@ -244,4 +244,4 @@ BoundingBox eigenbox_2d(const IT begin, const IT end) } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__EIGENBOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp index 9b8991a7c7132..07fd6c989cedc 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/lfit.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/lfit.hpp @@ -20,10 +20,10 @@ // cspell: ignore LFIT, lfit // LFIT means "L-Shape Fitting" -#ifndef GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#define GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ -#include +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" #include #include @@ -278,4 +278,4 @@ BoundingBox lfit_bounding_box_2d(const IT begin, const IT end) } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__LFIT_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__LFIT_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp index 5bc05810bb1b0..fb75384eb07cb 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/rotating_calipers.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box/rotating_calipers.hpp @@ -17,11 +17,11 @@ /// \file /// \brief This file implements the rotating calipers algorithm for minimum oriented bounding boxes -#ifndef GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#define GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ -#include -#include -#include +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include #include @@ -277,4 +277,4 @@ BoundingBox minimum_perimeter_bounding_box(std::list & list) } // namespace geometry } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX__ROTATING_CALIPERS_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp similarity index 71% rename from common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp index d1d84122889c9..c4c52928ac19a 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/bounding_box_2d.hpp @@ -15,12 +15,12 @@ // Co-developed by Tier IV, Inc. and Apex.AI, Inc. /// \file /// \brief Main header for user-facing bounding box algorithms: functions and types -#ifndef GEOMETRY__BOUNDING_BOX_2D_HPP_ -#define GEOMETRY__BOUNDING_BOX_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ -#include -#include -#include +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" +#include "autoware_auto_geometry/bounding_box/lfit.hpp" +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" namespace autoware { @@ -31,4 +31,4 @@ namespace geometry } // namespace geometry } // namespace common } // namespace autoware -#endif // GEOMETRY__BOUNDING_BOX_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__BOUNDING_BOX_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_2d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/common_2d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp index fd045003521ea..e3c2e58c9f80e 100644 --- a/common/autoware_auto_geometry/include/geometry/common_2d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_2d.hpp @@ -16,12 +16,12 @@ /// \file /// \brief This file includes common functionality for 2D geometry, such as dot products -#ifndef GEOMETRY__COMMON_2D_HPP_ -#define GEOMETRY__COMMON_2D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" -#include +#include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -584,4 +584,4 @@ bool is_point_inside_polygon_2d( } // namespace common } // namespace autoware -#endif // GEOMETRY__COMMON_2D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_2D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/common_3d.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp similarity index 93% rename from common/autoware_auto_geometry/include/geometry/common_3d.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp index 4477b010e7eba..74cd210dec586 100644 --- a/common/autoware_auto_geometry/include/geometry/common_3d.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/common_3d.hpp @@ -16,10 +16,10 @@ /// \file /// \brief This file includes common functionality for 3D geometry, such as dot products -#ifndef GEOMETRY__COMMON_3D_HPP_ -#define GEOMETRY__COMMON_3D_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ -#include +#include "autoware_auto_geometry/common_2d.hpp" namespace autoware { @@ -74,4 +74,4 @@ inline OUT distance_3d(const T1 & a, const T2 & b) } // namespace common } // namespace autoware -#endif // GEOMETRY__COMMON_3D_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__COMMON_3D_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/convex_hull.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp index e690c4d07441b..ae81c55ad6b55 100644 --- a/common/autoware_auto_geometry/include/geometry/convex_hull.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/convex_hull.hpp @@ -18,11 +18,12 @@ /// \brief This file implements the monotone chain algorithm to compute 2D convex hulls on linked /// lists of points -#ifndef GEOMETRY__CONVEX_HULL_HPP_ -#define GEOMETRY__CONVEX_HULL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" + +#include // lint -e537 NOLINT pclint vs cpplint #include @@ -191,4 +192,4 @@ typename std::list::const_iterator convex_hull(std::list & list) } // namespace common } // namespace autoware -#endif // GEOMETRY__CONVEX_HULL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__CONVEX_HULL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp similarity index 94% rename from common/autoware_auto_geometry/include/geometry/hull_pockets.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp index 6e8caa0df1e80..cd9fd49f71635 100644 --- a/common/autoware_auto_geometry/include/geometry/hull_pockets.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/hull_pockets.hpp @@ -18,11 +18,12 @@ /// \brief This file implements an algorithm for getting a list of "pockets" in the convex /// hull of a non-convex simple polygon. -#ifndef GEOMETRY__HULL_POCKETS_HPP_ -#define GEOMETRY__HULL_POCKETS_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" + +#include #include #include @@ -107,4 +108,4 @@ typename std::vector::value_typ } // namespace common } // namespace autoware -#endif // GEOMETRY__HULL_POCKETS_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__HULL_POCKETS_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/intersection.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/intersection.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp index 87dc32b0190d0..08c70c3a5a6be 100644 --- a/common/autoware_auto_geometry/include/geometry/intersection.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/intersection.hpp @@ -14,11 +14,11 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef GEOMETRY__INTERSECTION_HPP_ -#define GEOMETRY__INTERSECTION_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ -#include -#include +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include #include @@ -309,4 +309,4 @@ common::types::float32_t convex_intersection_over_union_2d( } // namespace common } // namespace autoware -#endif // GEOMETRY__INTERSECTION_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__INTERSECTION_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/interval.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/interval.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp index 59c26f27cc454..54be2c32b1d05 100644 --- a/common/autoware_auto_geometry/include/geometry/interval.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/interval.hpp @@ -18,11 +18,11 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#ifndef GEOMETRY__INTERVAL_HPP_ -#define GEOMETRY__INTERVAL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ -#include "common/types.hpp" -#include "helper_functions/float_comparisons.hpp" +#include +#include #include #include @@ -355,4 +355,4 @@ T Interval::clamp_to(const Interval & i, T val) } // namespace common } // namespace autoware -#endif // GEOMETRY__INTERVAL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__INTERVAL_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp similarity index 96% rename from common/autoware_auto_geometry/include/geometry/lookup_table.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp index e23a8c31b60f8..7b8867ca096ae 100644 --- a/common/autoware_auto_geometry/include/geometry/lookup_table.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/lookup_table.hpp @@ -17,11 +17,12 @@ /// \file /// \brief This file contains a 1D linear lookup table implementation -#ifndef GEOMETRY__LOOKUP_TABLE_HPP_ -#define GEOMETRY__LOOKUP_TABLE_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ -#include "common/types.hpp" -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" + +#include #include #include @@ -175,4 +176,4 @@ class LookupTable1D } // namespace common } // namespace autoware -#endif // GEOMETRY__LOOKUP_TABLE_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__LOOKUP_TABLE_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp similarity index 97% rename from common/autoware_auto_geometry/include/geometry/spatial_hash.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp index 78626548e5c74..8936e2c17d779 100644 --- a/common/autoware_auto_geometry/include/geometry/spatial_hash.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash.hpp @@ -17,12 +17,13 @@ /// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in /// 2D -#ifndef GEOMETRY__SPATIAL_HASH_HPP_ -#define GEOMETRY__SPATIAL_HASH_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ -#include -#include -#include +#include "autoware_auto_geometry/spatial_hash_config.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" + +#include #include #include @@ -328,4 +329,4 @@ using SpatialHash3d = SpatialHash; } // namespace common } // namespace autoware -#endif // GEOMETRY__SPATIAL_HASH_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp similarity index 98% rename from common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp index e118ec24c7759..24c4d6e879d4a 100644 --- a/common/autoware_auto_geometry/include/geometry/spatial_hash_config.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/spatial_hash_config.hpp @@ -17,14 +17,14 @@ /// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in /// 2D -#ifndef GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ -#include "helper_functions/crtp.hpp" +#include "autoware_auto_geometry/common_2d.hpp" +#include "autoware_auto_geometry/visibility_control.hpp" -#include -#include -#include +#include +#include #include #include @@ -447,4 +447,4 @@ class GEOMETRY_PUBLIC Config3d : public Config } // namespace common } // namespace autoware -#endif // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__SPATIAL_HASH_CONFIG_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp similarity index 90% rename from common/autoware_auto_geometry/include/geometry/visibility_control.hpp rename to common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp index 96efe9aa6e27b..8972246997997 100644 --- a/common/autoware_auto_geometry/include/geometry/visibility_control.hpp +++ b/common/autoware_auto_geometry/include/autoware_auto_geometry/visibility_control.hpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#ifndef GEOMETRY__VISIBILITY_CONTROL_HPP_ -#define GEOMETRY__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -38,4 +38,4 @@ #else // defined(__linux__) #error "Unsupported Build Configuration" #endif // defined(__WIN32) -#endif // GEOMETRY__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE_AUTO_GEOMETRY__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_auto_geometry/src/bounding_box.cpp b/common/autoware_auto_geometry/src/bounding_box.cpp index 225ea099e4e79..3a4ea96a151a2 100644 --- a/common/autoware_auto_geometry/src/bounding_box.cpp +++ b/common/autoware_auto_geometry/src/bounding_box.cpp @@ -14,13 +14,14 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. +#include "autoware_auto_geometry/bounding_box/bounding_box_common.hpp" +#include "autoware_auto_geometry/bounding_box/eigenbox_2d.hpp" + #include -#include -#include // cspell: ignore eigenbox -#include +#include "autoware_auto_geometry/bounding_box/lfit.hpp" // cspell: ignore lfit -#include +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" #include diff --git a/common/autoware_auto_geometry/src/spatial_hash.cpp b/common/autoware_auto_geometry/src/spatial_hash.cpp index 024cca48b8ee7..ffd91aaa08b3a 100644 --- a/common/autoware_auto_geometry/src/spatial_hash.cpp +++ b/common/autoware_auto_geometry/src/spatial_hash.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/spatial_hash.hpp" #include // lint -e537 NOLINT repeated include file due to cpplint rule diff --git a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp index 5e42622b19ce9..a179fbde5f151 100644 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp @@ -17,9 +17,9 @@ #ifndef TEST_BOUNDING_BOX_HPP_ #define TEST_BOUNDING_BOX_HPP_ -#include "geometry/bounding_box/lfit.hpp" +#include "autoware_auto_geometry/bounding_box/lfit.hpp" // cspell: ignore lfit -#include "geometry/bounding_box/rotating_calipers.hpp" +#include "autoware_auto_geometry/bounding_box/rotating_calipers.hpp" #include diff --git a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp index 50e946c416270..fc2d97c257b95 100644 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp @@ -17,7 +17,7 @@ #ifndef TEST_SPATIAL_HASH_HPP_ #define TEST_SPATIAL_HASH_HPP_ -#include "geometry/spatial_hash.hpp" +#include "autoware_auto_geometry/spatial_hash.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/lookup_table.cpp b/common/autoware_auto_geometry/test/src/lookup_table.cpp index e7533518d7f49..81865656c55b5 100644 --- a/common/autoware_auto_geometry/test/src/lookup_table.cpp +++ b/common/autoware_auto_geometry/test/src/lookup_table.cpp @@ -14,8 +14,9 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_geometry/lookup_table.hpp" + +#include #include diff --git a/common/autoware_auto_geometry/test/src/test_area.cpp b/common/autoware_auto_geometry/test/src/test_area.cpp index d722314dada83..bc9c28682ed44 100644 --- a/common/autoware_auto_geometry/test/src/test_area.cpp +++ b/common/autoware_auto_geometry/test/src/test_area.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/common_2d.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_common_2d.cpp b/common/autoware_auto_geometry/test/src/test_common_2d.cpp index 642e396bdce31..baf6967edd47e 100644 --- a/common/autoware_auto_geometry/test/src/test_common_2d.cpp +++ b/common/autoware_auto_geometry/test/src/test_common_2d.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include +#include "autoware_auto_geometry/common_2d.hpp" #include #include diff --git a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp index 43e3a3ad08adf..8b9bbce36c428 100644 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp @@ -14,7 +14,7 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "geometry/convex_hull.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp index 2b79d4ff0f22b..9477a83a488ed 100644 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp @@ -14,8 +14,8 @@ // // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include "geometry/convex_hull.hpp" -#include "geometry/hull_pockets.hpp" +#include "autoware_auto_geometry/convex_hull.hpp" +#include "autoware_auto_geometry/hull_pockets.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_intersection.cpp b/common/autoware_auto_geometry/test/src/test_intersection.cpp index 69c54960d4fc5..1036c69573c49 100644 --- a/common/autoware_auto_geometry/test/src/test_intersection.cpp +++ b/common/autoware_auto_geometry/test/src/test_intersection.cpp @@ -14,8 +14,8 @@ // Co-developed by Tier IV, Inc. and Apex.AI, Inc. -#include -#include +#include "autoware_auto_geometry/convex_hull.hpp" +#include "autoware_auto_geometry/intersection.hpp" #include diff --git a/common/autoware_auto_geometry/test/src/test_interval.cpp b/common/autoware_auto_geometry/test/src/test_interval.cpp index ba8d5742dadc5..266ab123f5203 100644 --- a/common/autoware_auto_geometry/test/src/test_interval.cpp +++ b/common/autoware_auto_geometry/test/src/test_interval.cpp @@ -18,7 +18,7 @@ // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. -#include "geometry/interval.hpp" +#include "autoware_auto_geometry/interval.hpp" #include 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 75% 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 5d99b8a463711..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,13 +12,17 @@ // 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 #include #include @@ -82,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 @@ -98,14 +104,25 @@ get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr( const std::string & uuid, const geometry_msgs::msg::Point & centroid, 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( @@ -120,7 +137,24 @@ get_acceleration_text_marker_ptr( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_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( @@ -133,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); @@ -232,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 73% 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 10bf11847c2f5..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 "rviz_common/properties/enum_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 #include @@ -64,19 +64,32 @@ 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{ "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization", this}, + + m_display_existence_probability_property{ + "Display Existence Probability", false, "Enable/disable existence probability visualization", + this}, + m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, m_default_topic{default_topic} { @@ -91,6 +104,14 @@ 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) { const auto & class_property_values = map_property_it.second; @@ -164,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; } @@ -181,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 @@ -201,6 +225,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return std::nullopt; } } + template + std::optional get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const ClassificationContainerT & labels) const + { + if (m_display_existence_probability_property.getBool()) { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + return detail::get_existence_probability_marker_ptr( + centroid, orientation, existence_probability, color_rgba); + } else { + return std::nullopt; + } + } template std::optional get_uuid_marker_ptr( @@ -216,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; } @@ -254,10 +303,49 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const { if (m_display_twist_property.getBool()) { - return detail::get_twist_marker_ptr(pose_with_covariance, twist_with_covariance); + return detail::get_twist_marker_ptr(pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + + 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; } @@ -324,7 +412,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } return (it->second).label; } - std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const { std::stringstream ss; @@ -386,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; @@ -397,22 +524,35 @@ 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 rviz_common::properties::BoolProperty m_display_path_confidence_property; + + rviz_common::properties::BoolProperty m_display_existence_probability_property; + // Property to decide line width of object shape rviz_common::properties::FloatProperty m_line_width_property; // Default topic name to be visualized @@ -424,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 78% 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 9f00a2cb1cde2..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 @@ -39,11 +39,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay Q_OBJECT public: + using TrackedObject = autoware_auto_perception_msgs::msg::TrackedObject; using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; TrackedObjectsDisplay(); +protected: + uint get_object_dynamics_to_visualize() + { + return m_select_object_dynamics_property->getOptionInt(); + } + + static bool is_object_to_show(const uint showing_dynamic_status, const TrackedObject & object); + private: + // Property to choose object dynamics to visualize + rviz_common::properties::EnumProperty * m_select_object_dynamics_property; + void processMessage(TrackedObjects::ConstSharedPtr msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) @@ -102,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 ac9c5af4ddeef..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 +#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 067173288e685..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,43 @@ 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; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = id++; + add_marker(existence_prob_marker_ptr); + } + // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5; @@ -73,13 +112,46 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + 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 = 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 894e377a6f851..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 @@ -56,7 +57,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_ptr->ns = std::string("path confidence"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->scale.x = 0.5; marker_ptr->scale.y = 0.5; marker_ptr->scale.z = 0.5; @@ -77,7 +78,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; marker_ptr->ns = std::string("path"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->pose = initPose(); marker_ptr->color = predicted_path_color; marker_ptr->color.a = 0.6; @@ -91,28 +92,27 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_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_LIST; marker_ptr->ns = std::string("twist"); - marker_ptr->scale.x = 0.03; + 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 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.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.0; @@ -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) @@ -137,7 +299,7 @@ visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( marker_ptr->text = std::to_string(static_cast(vel * 3.6)) + std::string("[km/h]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color = color_rgba; return marker_ptr; } @@ -158,52 +320,122 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( marker_ptr->text = getRoundedDoubleString(acc) + std::string("[m/s^2]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color = color_rgba; 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_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); - marker_ptr->color.a = 0.999; + + // marker configuration + 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 = 1.0; marker_ptr->color.b = 1.0; @@ -237,6 +469,23 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( marker_ptr->text = label; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color = color_rgba; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("existence probability"); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.z = 0.5; + marker_ptr->text = std::to_string(existence_probability); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); marker_ptr->color = color_rgba; return marker_ptr; @@ -245,7 +494,8 @@ visualization_msgs::msg::Marker::SharedPtr get_label_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"); @@ -254,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); @@ -267,7 +520,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; @@ -277,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"); @@ -286,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); @@ -299,170 +556,127 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; 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 2cc5397d18721..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,16 +111,46 @@ 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 + geometry_msgs::msg::Point existence_probability_position; + existence_probability_position.x = + object.kinematics.initial_pose_with_covariance.pose.position.x + 0.5; + existence_probability_position.y = + object.kinematics.initial_pose_with_covariance.pose.position.y; + existence_probability_position.z = + object.kinematics.initial_pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, + object.kinematics.initial_pose_with_covariance.pose.orientation, existence_probability, + object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; @@ -154,7 +184,7 @@ std::vector PredictedObjectsDisplay: // Get marker for twist auto twist_marker = get_twist_marker_ptr( object.kinematics.initial_pose_with_covariance, - object.kinematics.initial_twist_with_covariance); + object.kinematics.initial_twist_with_covariance, get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; @@ -162,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 00a1199c697ce..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 @@ -26,6 +26,24 @@ namespace object_detection { TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") { + // Option for selective visualization by object dynamics + m_select_object_dynamics_property = new rviz_common::properties::EnumProperty( + "Dynamic Status", "All", "Selectively visualize objects by its dynamic status.", this); + m_select_object_dynamics_property->addOption("Dynamic", 0); + m_select_object_dynamics_property->addOption("Static", 1); + m_select_object_dynamics_property->addOption("All", 2); +} + +bool TrackedObjectsDisplay::is_object_to_show( + const uint showing_dynamic_status, const TrackedObject & object) +{ + if (showing_dynamic_status == 0 && object.kinematics.is_stationary) { + return false; // Show only moving objects + } + if (showing_dynamic_status == 1 && !object.kinematics.is_stationary) { + return false; // Show only stationary objects + } + return true; } void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) @@ -33,12 +51,17 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) clear_markers(); update_id_map(msg); + const auto showing_dynamic_status = get_object_dynamics_to_visualize(); for (const auto & object : msg->objects) { + // Filter by object dynamic status + if (!is_object_to_show(showing_dynamic_status, object)) continue; // 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, - 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; @@ -72,16 +95,42 @@ 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; + existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y; + existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5; + const float existence_probability = object.existence_probability; + auto existence_prob_marker = get_existence_probability_marker_ptr( + existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation, + existence_probability, object.classification); + if (existence_prob_marker) { + auto existence_prob_marker_ptr = existence_prob_marker.value(); + existence_prob_marker_ptr->header = msg->header; + existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(existence_prob_marker_ptr); + } // Get marker for velocity text geometry_msgs::msg::Point vel_vis_position; vel_vis_position.x = uuid_vis_position.x - 0.5; @@ -113,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); + 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/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index b35eb6e93ce6e..50c16ba8eaf7d 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -17,7 +17,7 @@ #ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ #define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#include +#include #include #include 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/include/component_interface_utils/rclcpp/interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp index bc48e2a0294e0..97f46933f2fe6 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp @@ -52,7 +52,7 @@ struct NodeInterface {ServiceLog::CLIENT_RESPONSE, "client exit"}, {ServiceLog::ERROR_UNREADY, "client unready"}, {ServiceLog::ERROR_TIMEOUT, "client timeout"}}); - RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name); + RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name); ServiceLog msg; msg.stamp = node->now(); 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/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index d66d0fed3033c..e065f332b75e4 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,7 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include +#include #include #include 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/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp index 516b3ab09e8b7..8c1d49fb5f607 100644 --- a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -39,6 +39,10 @@ std::vector slerp( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); + +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio); } // namespace interpolation #endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index c3595d212f349..e44626498a80b 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -57,4 +57,15 @@ std::vector slerp( return query_values; } +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} } // namespace interpolation diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index 43c8158b2f98b..f42deaa7f8c41 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -14,7 +14,7 @@ ament_auto_add_library(motion_utils SHARED src/trajectory/trajectory.cpp src/trajectory/interpolation.cpp src/trajectory/path_with_lane_id.cpp - src/trajectory/tmp_conversion.cpp + src/trajectory/conversion.cpp src/vehicle/vehicle_state_checker.cpp ) diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 95bbcd93e30df..be5a70ed7ffc9 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -20,9 +20,11 @@ #include #include +#include #include #include #include + namespace motion_utils { @@ -38,7 +40,7 @@ struct VirtualWall double longitudinal_offset{}; bool is_driving_forward{true}; }; -typedef std::vector VirtualWalls; +using VirtualWalls = std::vector; /// @brief class to manage the creation of virtual wall markers /// @details creates both ADD and DELETE markers @@ -55,8 +57,8 @@ class VirtualWallMarkerCreator const rclcpp::Time & now, const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, const bool is_driving_forward)>; - VirtualWalls virtual_walls; - std::unordered_map marker_count_per_namespace; + VirtualWalls virtual_walls_; + std::unordered_map marker_count_per_namespace_; /// @brief internal cleanup: clear the stored markers and remove unused namespace from the map void cleanup(); diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 106ed32b18576..258386ca236a3 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,13 +15,10 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include -#include -#include #include namespace motion_utils @@ -187,7 +184,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, - const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); + const bool use_zero_order_hold_for_twist = true, + const bool resample_input_path_stop_point = true); /** * @brief A resampling function for a trajectory. Note that in a default setting, position xy are diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 1e4d0d633325e..8bb5f13e3fb78 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#include "tier4_autoware_utils/system/backtrace.hpp" + #include #include #include @@ -23,26 +25,21 @@ namespace resample_utils { -constexpr double CLOSE_S_THRESHOLD = 1e-6; +constexpr double close_s_threshold = 1e-6; + +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; template bool validate_size(const T & points) { - if (points.size() < 2) { - return false; - } - return true; + return points.size() >= 2; } template bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) { const double points_length = motion_utils::calcArcLength(points); - if (points_length < resampling_intervals.back()) { - return false; - } - - return true; + return points_length >= resampling_intervals.back(); } template @@ -52,7 +49,7 @@ bool validate_points_duplication(const T & points) const auto & curr_pt = tier4_autoware_utils::getPoint(points.at(i)); const auto & next_pt = tier4_autoware_utils::getPoint(points.at(i + 1)); const double ds = tier4_autoware_utils::calcDistance2d(curr_pt, next_pt); - if (ds < CLOSE_S_THRESHOLD) { + if (ds < close_s_threshold) { return false; } } @@ -65,22 +62,28 @@ bool validate_arguments(const T & input_points, const std::vector & resa { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; - } else if (!validate_size(resampling_intervals)) { - std::cerr << "The number of resampling intervals is less than 2" << std::endl; + } + if (!validate_size(resampling_intervals)) { + log_error( + "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { - std::cerr << "resampling interval is longer than input points" << std::endl; + log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } @@ -92,20 +95,24 @@ bool validate_arguments(const T & input_points, const double resampling_interval { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // check resampling interval if (resampling_interval < motion_utils::overlap_threshold) { - std::cerr << "resampling interval is less than " << motion_utils::overlap_threshold - << std::endl; + log_error( + "[resample_utils] invalid argument: resampling interval is less than " + + std::to_string(motion_utils::overlap_threshold)); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp new file mode 100644 index 0000000000000..7d4be216e89fe --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -0,0 +1,121 @@ +// 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 MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ + +#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "std_msgs/msg/header.hpp" + +#include + +namespace motion_utils +{ +using TrajectoryPoints = std::vector; + +/** + * @brief Convert std::vector to + * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header = std_msgs::msg::Header{}); + +/** + * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + +template +autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); + throw std::logic_error("Only specializations of convertToPath can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::Path convertToPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + autoware_auto_planning_msgs::msg::Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + +template +TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); + throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); +} + +template <> +inline TrajectoryPoints convertToTrajectoryPoints( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +{ + TrajectoryPoints output{}; + for (const auto & p : input.points) { + autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + output.emplace_back(tp); + } + return output; +} + +template +autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + [[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); + throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); +} + +template <> +inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + const TrajectoryPoints & input) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + for (const auto & p : input) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + output.points.emplace_back(pp); + } + return output; +} + +} // namespace motion_utils + +#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index c23478605c41a..aec329f9f540e 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -16,16 +16,12 @@ #define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include #include -#include -#include -#include namespace motion_utils { diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index e088285147735..d0ed761b76d9a 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include #include diff --git a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp deleted file mode 100644 index 6921c56e0199e..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ /dev/null @@ -1,48 +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 MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ - -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - -#include - -namespace motion_utils -{ -/** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory); - -/** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); - -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index cb815f7dd0a0c..b9136bc4002e3 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -30,10 +30,12 @@ #include #include #include +#include #include #include namespace motion_utils { +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; /** * @brief validate if points container is empty or not @@ -44,7 +46,7 @@ void validateNonEmpty(const T & points) { if (points.empty()) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Points is empty."); + throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); } } @@ -82,7 +84,7 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Sharp angle."); + throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -130,11 +132,11 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) if (points_with_twist.size() == 1) { if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { return true; - } else if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { + } + if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { return false; - } else { - return std::nullopt; } + return std::nullopt; } return isDrivingForward(points_with_twist); @@ -214,7 +216,7 @@ std::optional searchZeroVelocityIndex( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -246,7 +248,7 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -336,7 +338,7 @@ std::optional findNearestIndex( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -401,12 +403,17 @@ double calcLongitudinalOffsetToSegment( const bool throw_exception = false) { if (seg_idx >= points.size() - 1) { - const std::out_of_range e("Segment index is invalid."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Failed to calculate longitudinal offset because the given segment index is out of the " + "points size."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -418,18 +425,22 @@ double calcLongitudinalOffsetToSegment( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return std::nan(""); } } if (seg_idx >= overlap_removed_points.size() - 1) { - const std::runtime_error e("Same points are given."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Longitudinal offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -575,18 +586,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const std::runtime_error e("Same points are given."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -637,18 +654,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const std::runtime_error e("Same points are given."); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -684,7 +707,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -727,7 +750,7 @@ std::vector calcSignedArcLengthPartialSum( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -779,7 +802,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -822,7 +845,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -861,7 +884,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -901,7 +924,7 @@ double calcArcLength(const T & points) try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -929,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)); @@ -968,7 +994,7 @@ std::vector> calcCurvatureAndArcLength(const T & point { // Note that arclength is for the segment, not the sum. std::vector> curvature_arc_length_vec; - curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + curvature_arc_length_vec.emplace_back(0.0, 0.0); for (size_t i = 1; i < points.size() - 1; ++i) { const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); @@ -976,9 +1002,9 @@ std::vector> calcCurvatureAndArcLength(const T & point const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - curvature_arc_length_vec.push_back(std::pair(curvature, arc_length)); + curvature_arc_length_vec.emplace_back(curvature, arc_length); } - curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + curvature_arc_length_vec.emplace_back(0.0, 0.0); return curvature_arc_length_vec; } @@ -1007,7 +1033,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1041,17 +1067,22 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return {}; } @@ -1118,7 +1149,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } @@ -1166,21 +1197,25 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } if (points.size() - 1 < src_idx) { - const auto e = std::out_of_range("Invalid source index"); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::out_of_range(error_message); } - std::cerr << e.what() << std::endl; + log_error(error_message); return {}; } if (points.size() == 1) { + log_error("Failed to calculate longitudinal offset: points size is one."); return {}; } @@ -1265,7 +1300,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1311,7 +1346,7 @@ std::optional insertTargetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1326,7 +1361,7 @@ std::optional insertTargetPoint( try { validateNonSharpAngle(p_front, p_target, p_back); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -2041,9 +2076,8 @@ size_t findFirstNearestIndexWithSoftConstraints( if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { if (is_within_constraints) { break; - } else { - continue; } + continue; } if (min_squared_dist <= squared_dist) { @@ -2073,9 +2107,8 @@ size_t findFirstNearestIndexWithSoftConstraints( if (squared_dist_threshold < squared_dist) { if (is_within_constraints) { break; - } else { - continue; } + continue; } if (min_squared_dist <= squared_dist) { @@ -2194,7 +2227,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate stop distance" + std::string(e.what())); return {}; } @@ -2333,9 +2366,7 @@ T cropPoints( cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); if (cropped_points.size() < 2) { - RCLCPP_ERROR( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), - ". Return original points since cropped_points size is less than 2."); + log_error("Return original points since cropped_points size is less than 2."); return points; } @@ -2380,18 +2411,22 @@ double calcYawDeviation( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } } if (overlap_removed_points.size() <= 1) { - const std::runtime_error e("points size is less than 2"); + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " Given points size is less than 2. Failed to calculate yaw deviation."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { - throw e; + throw std::runtime_error(error_message); } - std::cerr << e.what() << std::endl; + log_error( + error_message + + " Return 0 since no_throw option is enabled. The maintainer must check the code."); return 0.0; } diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index 6a9bd01a5a9a0..82433d8e3c241 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -22,7 +22,6 @@ #include #include -#include namespace motion_utils { diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp index c6de95dd568cf..3ac37bbc7733b 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/motion_utils/src/distance/distance.cpp @@ -262,7 +262,8 @@ std::optional calcDecelDistWithJerkAndAccConstraints( if (t_during_min_acc > epsilon) { return calcDecelDistPlanType1( current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); - } else if (is_decel_needed || current_acc > epsilon) { + } + if (is_decel_needed || current_acc > epsilon) { return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); } diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 50def253b6d16..6e9c45adc241b 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,14 +14,12 @@ #include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample_utils.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include +#include -#include +#include -using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createDeletedDefaultMarker; using tier4_autoware_utils::createMarkerColor; diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index c22aaa0d5e87c..34aae096ffa9e 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -21,24 +21,24 @@ namespace motion_utils void VirtualWallMarkerCreator::cleanup() { - for (auto it = marker_count_per_namespace.begin(); it != marker_count_per_namespace.end();) { + for (auto it = marker_count_per_namespace_.begin(); it != marker_count_per_namespace_.end();) { const auto & marker_count = it->second; const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0; if (is_unused_namespace) - it = marker_count_per_namespace.erase(it); + it = marker_count_per_namespace_.erase(it); else ++it; } - virtual_walls.clear(); + virtual_walls_.clear(); } void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall) { - virtual_walls.push_back(virtual_wall); + virtual_walls_.push_back(virtual_wall); } void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls) { - virtual_walls.insert(virtual_walls.end(), walls.begin(), walls.end()); + virtual_walls_.insert(virtual_walls_.end(), walls.begin(), walls.end()); } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( @@ -46,13 +46,13 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( { visualization_msgs::msg::MarkerArray marker_array; // update marker counts - for (auto & [ns, count] : marker_count_per_namespace) { + for (auto & [ns, count] : marker_count_per_namespace_) { count.previous = count.current; count.current = 0UL; } // convert to markers create_wall_function create_fn; - for (const auto & virtual_wall : virtual_walls) { + for (const auto & virtual_wall : virtual_walls_) { switch (virtual_wall.style) { case stop: create_fn = motion_utils::createStopVirtualWallMarker; @@ -68,15 +68,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { - marker.id = marker_count_per_namespace[marker.ns].current++; + marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); } } // create delete markers visualization_msgs::msg::Marker marker; marker.action = visualization_msgs::msg::Marker::DELETE; - for (const auto & [ns, count] : marker_count_per_namespace) { - for (marker.id = count.current; marker.id < static_cast(count.previous); ++marker.id) { + for (const auto & [ns, count] : marker_count_per_namespace_) { + for (marker.id = static_cast(count.current); marker.id < static_cast(count.previous); + ++marker.id) { marker.ns = ns; marker_array.markers.push_back(marker); } diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 6bada921804d4..834d07a87ea09 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,15 +14,12 @@ #include "motion_utils/resample/resample.hpp" -#include "interpolation/interpolation_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" #include "motion_utils/resample/resample_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" namespace motion_utils { @@ -489,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = resampled_path.right_bound; + resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp similarity index 86% rename from common/motion_utils/src/trajectory/tmp_conversion.cpp rename to common/motion_utils/src/trajectory/conversion.cpp index 5c907a5926046..f198003d84091 100644 --- a/common/motion_utils/src/trajectory/tmp_conversion.cpp +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -12,10 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/tmp_conversion.hpp" - -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include @@ -33,15 +30,12 @@ namespace motion_utils * points larger than the capacity. (Tier IV) */ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory) + const std::vector & trajectory, + const std_msgs::msg::Header & header) { autoware_auto_planning_msgs::msg::Trajectory output{}; - for (const auto & pt : trajectory) { - output.points.push_back(pt); - if (output.points.size() >= output.CAPACITY) { - break; - } - } + output.header = header; + for (const auto & pt : trajectory) output.points.push_back(pt); return output; } diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index 4077202aba8a4..6ee3e665f746a 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -15,8 +15,6 @@ #include "motion_utils/trajectory/interpolation.hpp" #include "interpolation/linear_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" using autoware_auto_planning_msgs::msg::PathPointWithLaneId; @@ -34,7 +32,8 @@ TrajectoryPoint calcInterpolatedPoint( TrajectoryPoint interpolated_point{}; interpolated_point.pose = target_pose; return interpolated_point; - } else if (trajectory.points.size() == 1) { + } + if (trajectory.points.size() == 1) { return trajectory.points.front(); } @@ -101,7 +100,8 @@ PathPointWithLaneId calcInterpolatedPoint( PathPointWithLaneId interpolated_point{}; interpolated_point.point.pose = target_pose; return interpolated_point; - } else if (path.points.size() == 1) { + } + if (path.points.size() == 1) { return path.points.front(); } diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 80df1fabf562d..5794ed61e9e44 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 1786586ec39a8..eb6a06041e65d 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -803,17 +803,6 @@ TEST(trajectory, convertToTrajectory) const auto traj = convertToTrajectory(traj_input); EXPECT_EQ(traj.points.size(), traj_input.size()); } - - // Clipping check - { - const auto traj_input = generateTestTrajectoryPointArray(10000, 1.0); - const auto traj = convertToTrajectory(traj_input); - EXPECT_EQ(traj.points.size(), traj.CAPACITY); - // Value check - for (size_t i = 0; i < traj.points.size(); ++i) { - EXPECT_EQ(traj.points.at(i), traj_input.at(i)); - } - } } TEST(trajectory, convertToTrajectoryPointArray) @@ -1894,7 +1883,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1907,7 +1896,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1920,7 +1909,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -2321,7 +2310,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4328,7 +4317,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4341,7 +4330,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4354,7 +4343,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } 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/include/tier4_api_utils/rclcpp/client.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp index aedbe4b34ea22..16ff5a011536c 100644 --- a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp @@ -41,20 +41,20 @@ class Client const typename ServiceT::Request::SharedPtr & request, const std::chrono::nanoseconds & timeout = std::chrono::seconds(2)) { - RCLCPP_INFO(logger_, "client request"); + RCLCPP_DEBUG(logger_, "client request"); if (!client_->service_is_ready()) { - RCLCPP_INFO(logger_, "client available"); + RCLCPP_DEBUG(logger_, "client available"); return {response_error("Internal service is not available."), nullptr}; } auto future = client_->async_send_request(request); if (future.wait_for(timeout) != std::future_status::ready) { - RCLCPP_INFO(logger_, "client timeout"); + RCLCPP_DEBUG(logger_, "client timeout"); return {response_error("Internal service has timed out."), nullptr}; } - RCLCPP_INFO(logger_, "client response"); + RCLCPP_DEBUG(logger_, "client response"); return {response_success(), future.get()}; } 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_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt index c6d4e30061626..cdc57743a6cb1 100644 --- a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt +++ b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt @@ -11,6 +11,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED + src/automatic_checkpoint_append_tool.cpp src/automatic_goal_sender.cpp src/automatic_goal_append_tool.cpp src/automatic_goal_panel.cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticCheckpointTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml index deda078ace22e..fb5331379acb6 100644 --- a/common/tier4_automatic_goal_rviz_plugin/package.xml +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -6,6 +6,8 @@ The autoware automatic goal rviz plugin package Shumpei Wakabayashi Dawid Moszyński + Kyoichi Sugahara + Satoshi Ota Apache License 2.0 Dawid Moszyński @@ -22,6 +24,7 @@ rviz_default_plugins tf2 tf2_geometry_msgs + tier4_autoware_utils visualization_msgs yaml-cpp diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml index e7d5224550868..e9d77491941ed 100644 --- a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml @@ -9,4 +9,9 @@ base_class_type="rviz_common::Tool"> AutowareAutomaticGoalTool + + AutowareAutomaticCheckpointTool + diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp new file mode 100644 index 0000000000000..4efa6fedbaabd --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_checkpoint_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticCheckpointTool::AutowareAutomaticCheckpointTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/checkpoint", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticCheckpointTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendCheckpoint"); + updateTopic(); +} + +void AutowareAutomaticCheckpointTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticCheckpointTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticCheckpointTool"), + "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, position_z_->getFloat(), theta, + fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticCheckpointTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp new file mode 100644 index 0000000000000..4ea3fa4bd009a --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_checkpoint_append_tool.hpp @@ -0,0 +1,91 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ +#define AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticCheckpointTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticCheckpointTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_CHECKPOINT_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp index bee390bfe29ed..6b8d7765a989a 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -16,6 +16,8 @@ #include "automatic_goal_panel.hpp" +#include + namespace rviz_plugins { AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) @@ -139,6 +141,9 @@ void AutowareAutomaticGoalPanel::onInitialize() sub_append_goal_ = raw_node_->create_subscription( "~/automatic_goal/goal", 5, std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); + sub_append_checkpoint_ = raw_node_->create_subscription( + "~/automatic_goal/checkpoint", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendCheckpoint, this, std::placeholders::_1)); initCommunication(raw_node_.get()); } @@ -244,12 +249,28 @@ void AutowareAutomaticGoalPanel::onClickSaveListToFile() void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) { if (state_ == State::EDITING) { - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); updateGoalsList(); updateGUI(); } } +void AutowareAutomaticGoalPanel::onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose) +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList before set checkpoint"); + return; + } + + current_goal_ = goals_list_widget_ptr_->currentRow(); + if (current_goal_ >= goals_list_.size()) { + return; + } + + goals_list_.at(current_goal_).checkpoint_pose_ptrs.push_back(pose); + publishMarkers(); +} + // Override void AutowareAutomaticGoalPanel::onCallResult() { @@ -418,47 +439,71 @@ void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const void AutowareAutomaticGoalPanel::publishMarkers() { + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + MarkerArray text_array; MarkerArray arrow_array; // Clear existing - visualization_msgs::msg::Marker marker; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = 0; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - text_array.markers.push_back(marker); - marker.ns = "poses"; - arrow_array.markers.push_back(marker); - pub_marker_->publish(text_array); - pub_marker_->publish(arrow_array); + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::DELETEALL; + text_array.markers.push_back(marker); + pub_marker_->publish(text_array); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", 0L, Marker::CUBE, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + arrow_array.markers.push_back(marker); + pub_marker_->publish(arrow_array); + } + text_array.markers.clear(); arrow_array.markers.clear(); - // Publish current - for (unsigned i = 0; i < goals_list_.size(); i++) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Time(); - marker.ns = "names"; - marker.id = static_cast(i); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + + const auto push_arrow_marker = [&](const auto & pose, const auto & color, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "poses", id, Marker::ARROW, + createMarkerScale(1.6, 0.5, 0.5), color); marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = goals_list_[i]->pose; + marker.pose = pose; marker.lifetime = rclcpp::Duration(0, 0); - marker.scale.x = 1.6; - marker.scale.y = 1.6; - marker.scale.z = 1.6; - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.color.a = 1.0; marker.frame_locked = false; - marker.text = "G" + std::to_string(i); - text_array.markers.push_back(marker); - marker.ns = "poses"; - marker.scale.y = 0.2; - marker.scale.z = 0.2; - marker.type = visualization_msgs::msg::Marker::ARROW; arrow_array.markers.push_back(marker); + }; + + const auto push_text_marker = [&](const auto & pose, const auto & text, const size_t id) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "names", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(1.6, 1.6, 1.6), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = false; + marker.text = text; + text_array.markers.push_back(marker); + }; + + // Publish current + size_t id = 0; + for (size_t i = 0; i < goals_list_.size(); ++i) { + { + const auto pose = goals_list_.at(i).goal_pose_ptr->pose; + push_arrow_marker(pose, createMarkerColor(0.0, 1.0, 0.0, 0.999), id++); + push_text_marker(pose, "Goal:" + std::to_string(i), id++); + } + + for (size_t j = 0; j < goals_list_.at(i).checkpoint_pose_ptrs.size(); ++j) { + const auto pose = goals_list_.at(i).checkpoint_pose_ptrs.at(j)->pose; + push_arrow_marker(pose, createMarkerColor(1.0, 1.0, 0.0, 0.999), id++); + push_text_marker( + pose, "Checkpoint:" + std::to_string(i) + "[Goal:" + std::to_string(j) + "]", id++); + } } pub_marker_->publish(text_array); pub_marker_->publish(arrow_array); @@ -469,13 +514,13 @@ void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) { YAML::Node node; for (unsigned i = 0; i < goals_list_.size(); ++i) { - node[i]["position_x"] = goals_list_[i]->pose.position.x; - node[i]["position_y"] = goals_list_[i]->pose.position.y; - node[i]["position_z"] = goals_list_[i]->pose.position.z; - node[i]["orientation_x"] = goals_list_[i]->pose.orientation.x; - node[i]["orientation_y"] = goals_list_[i]->pose.orientation.y; - node[i]["orientation_z"] = goals_list_[i]->pose.orientation.z; - node[i]["orientation_w"] = goals_list_[i]->pose.orientation.w; + node[i]["position_x"] = goals_list_[i].goal_pose_ptr->pose.position.x; + node[i]["position_y"] = goals_list_[i].goal_pose_ptr->pose.position.y; + node[i]["position_z"] = goals_list_[i].goal_pose_ptr->pose.position.z; + node[i]["orientation_x"] = goals_list_[i].goal_pose_ptr->pose.orientation.x; + node[i]["orientation_y"] = goals_list_[i].goal_pose_ptr->pose.orientation.y; + node[i]["orientation_z"] = goals_list_[i].goal_pose_ptr->pose.orientation.z; + node[i]["orientation_w"] = goals_list_[i].goal_pose_ptr->pose.orientation.w; } std::ofstream file_out(file_path); file_out << node; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp index 0ec9ca530f074..72a5bd4fb80fe 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp @@ -61,7 +61,9 @@ class AutowareAutomaticGoalPanel : public rviz_common::Panel, public automatic_goal::AutowareAutomaticGoalSender { using State = automatic_goal::AutomaticGoalState; + using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; + using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; @@ -96,6 +98,7 @@ public Q_SLOTS: // NOLINT for Qt // Inputs void onAppendGoal(const PoseStamped::ConstSharedPtr pose); + void onAppendCheckpoint(const PoseStamped::ConstSharedPtr pose); // Visual updates void updateGUI(); @@ -116,6 +119,7 @@ public Q_SLOTS: // NOLINT for Qt // Pub/Sub rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_checkpoint_{nullptr}; // Containers rclcpp::Node::SharedPtr raw_node_{nullptr}; diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp index cc8d46e2f60c6..3ca368a3bd1a4 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -108,9 +108,9 @@ void AutowareAutomaticGoalSender::updateGoalsList() std::stringstream ss; ss << std::fixed << std::setprecision(2); tf2::Quaternion tf2_quat; - tf2::convert(goal->pose.orientation, tf2_quat); - ss << "G" << i << " (" << goal->pose.position.x << ", "; - ss << goal->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; + tf2::convert(goal.goal_pose_ptr->pose.orientation, tf2_quat); + ss << "G" << i << " (" << goal.goal_pose_ptr->pose.position.x << ", "; + ss << goal.goal_pose_ptr->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); } onGoalListUpdated(); @@ -178,7 +178,7 @@ void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) pose->pose.orientation.y = goal["orientation_y"].as(); pose->pose.orientation.z = goal["orientation_z"].as(); pose->pose.orientation.w = goal["orientation_w"].as(); - goals_list_.push_back(pose); + goals_list_.emplace_back(pose); } resetAchievedGoals(); updateGoalsList(); diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp index aacdada352811..88b7b5e7dac20 100644 --- a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp @@ -80,8 +80,11 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } auto req = std::make_shared(); - req->header = goals_list_.at(goal_index)->header; - req->goal = goals_list_.at(goal_index)->pose; + req->header = goals_list_.at(goal_index).goal_pose_ptr->header; + req->goal = goals_list_.at(goal_index).goal_pose_ptr->pose; + for (const auto & checkpoint : goals_list_.at(goal_index).checkpoint_pose_ptrs) { + req->waypoints.push_back(checkpoint->pose); + } client->async_send_request( req, [this](typename rclcpp::Client::SharedFuture result) { if (result.get()->status.code != 0) state_ = State::ERROR; @@ -120,6 +123,13 @@ class AutowareAutomaticGoalSender : public rclcpp::Node } } + struct Route + { + explicit Route(const PoseStamped::ConstSharedPtr & goal) : goal_pose_ptr{goal} {} + PoseStamped::ConstSharedPtr goal_pose_ptr{}; + std::vector checkpoint_pose_ptrs{}; + }; + // Update void updateGoalsList(); virtual void updateAutoExecutionTimerTick(); @@ -155,7 +165,7 @@ class AutowareAutomaticGoalSender : public rclcpp::Node // Containers unsigned current_goal_{0}; State state_{State::INITIALIZING}; - std::vector goals_list_{}; + std::vector goals_list_{}; std::map> goals_achieved_{}; std::string goals_achieved_file_path_{}; diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index 9610bf1e2b7f1..fdd270fcce2ef 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -263,9 +263,9 @@ void ManualController::onClickEnableButton() { auto req = std::make_shared(); req->engage = true; - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client_engage_->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client_engage_->async_send_request( 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/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index 11dd050658aeb..a298967a31af9 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -38,9 +38,21 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance - behavior_path_planner_lane_change: + behavior_path_planner_goal_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner + + behavior_path_planner_start_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.start_planner + + behavior_path_avoidance_by_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE + + behavior_path_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change.NORMAL behavior_velocity_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index 5c17af5df22d1..69b61ba506b32 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -18,13 +18,13 @@ include_directories( ament_auto_add_library(tier4_planning_rviz_plugin SHARED # path point - include/path/display_base.hpp - include/path/display.hpp + include/tier4_planning_rviz_plugin/path/display_base.hpp + include/tier4_planning_rviz_plugin/path/display.hpp src/path/display.cpp # footprint - include/pose_with_uuid_stamped/display.hpp + include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp src/pose_with_uuid_stamped/display.cpp - include/mission_checkpoint/mission_checkpoint.hpp + include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp src/mission_checkpoint/mission_checkpoint.cpp src/tools/jsk_overlay_utils.cpp src/tools/max_velocity.cpp diff --git a/common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp similarity index 93% rename from common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp index ea15788d789fe..964c418a3df1f 100644 --- a/common/tier4_planning_rviz_plugin/include/mission_checkpoint/mission_checkpoint.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp @@ -45,8 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ -#define MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 #include @@ -88,4 +88,4 @@ private Q_SLOTS: } // namespace rviz_plugins -#endif // MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__MISSION_CHECKPOINT__MISSION_CHECKPOINT_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp similarity index 97% rename from common/tier4_planning_rviz_plugin/include/path/display.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index 7d635f0cedcad..66b4a31f0993f 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH__DISPLAY_HPP_ -#define PATH__DISPLAY_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ -#include +#include "tier4_planning_rviz_plugin/path/display_base.hpp" #include #include @@ -228,4 +228,4 @@ class AutowareTrajectoryDisplay }; } // namespace rviz_plugins -#endif // PATH__DISPLAY_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp similarity index 99% rename from common/tier4_planning_rviz_plugin/include/path/display_base.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index 74c2a60df3f32..4a59006e3bb96 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH__DISPLAY_BASE_HPP_ -#define PATH__DISPLAY_BASE_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ #include #include @@ -40,9 +40,10 @@ #include #define EIGEN_MPL2_ONLY +#include "tier4_planning_rviz_plugin/utils.hpp" + #include #include -#include namespace { @@ -654,4 +655,4 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay }; } // namespace rviz_plugins -#endif // PATH__DISPLAY_BASE_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp similarity index 92% rename from common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp index f1fe3c30cd457..285a6902ccc1a 100644 --- a/common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ -#define POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ #include @@ -81,4 +81,4 @@ private Q_SLOTS: } // namespace rviz_plugins -#endif // POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/utils.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp similarity index 93% rename from common/tier4_planning_rviz_plugin/include/utils.hpp rename to common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp index 76f9da0685908..94943a25f6a64 100644 --- a/common/tier4_planning_rviz_plugin/include/utils.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS_HPP_ -#define UTILS_HPP_ +#ifndef TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ +#define TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ #include #include @@ -61,4 +61,4 @@ bool isDrivingForward(const T points_with_twist, size_t target_idx) } } // namespace rviz_plugins -#endif // UTILS_HPP_ +#endif // TIER4_PLANNING_RVIZ_PLUGIN__UTILS_HPP_ diff --git a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp index 6be2cfc426870..2ba5a309baa5c 100644 --- a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp +++ b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp @@ -45,7 +45,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "tier4_planning_rviz_plugin/mission_checkpoint/mission_checkpoint.hpp" #ifdef ROS_DISTRO_GALACTIC #include diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 5bda4cdafd7e3..916201669e9e3 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "tier4_planning_rviz_plugin/path/display.hpp" + #include namespace rviz_plugins diff --git a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp index 8df04e04b2963..b525608a65625 100644 --- a/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rviz_common/properties/tf_frame_property.hpp" +#include "tier4_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp" -#include #include +#include #include #include #include diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index eba3e4eacd275..8f67a90215bd1 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -171,15 +171,15 @@ public Q_SLOTS: // NOLINT for Qt { auto req = std::make_shared(); - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_INFO( + RCLCPP_DEBUG( raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); }); diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index e27fc9b6cfa81..6ea142ed66a1b 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -174,9 +174,6 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: case SteeringFactor::APPROACHING: label->setText("APPROACHING"); break; - case SteeringFactor::TRYING: - label->setText("TRYING"); - break; case SteeringFactor::TURNING: label->setText("TURNING"); break; diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 081eab28e8833..6ce95acc4ef1d 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,10 +15,13 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#include "autoware_perception_msgs/msg/traffic_signal.hpp" +#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" #include "tier4_perception_msgs/msg/traffic_signal.hpp" +#include #include #include #include @@ -36,6 +39,47 @@ bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal); void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence = -1); +/** + * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. + * + * Iterates through the traffic light elements to find a circle-shaped light that matches the given + * color. + * + * @param tl_state The traffic light state to check. + * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. + * @return True if a circle-shaped light with the specified color is found, false otherwise. + */ +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + +/** + * @brief Checks if a traffic light state includes a light with the specified shape. + * + * Searches through the traffic light elements to find a light that matches the given shape. + * + * @param tl_state The traffic light state to check. + * @param shape The shape to look for in the traffic light's lights. + * @return True if a light with the specified shape is found, false otherwise. + */ +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param tl_state The current state of the traffic light associated with the lanelet. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state); + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); tf2::Vector3 getTrafficLightBottomRight(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index de05355eafd66..37b4d46ce356a 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -6,6 +6,7 @@ The traffic_light_utils package Mingyu Li Shunsuke Miura + Satoshi Ota Apache License 2.0 ament_cmake_auto @@ -15,6 +16,7 @@ ament_lint_auto autoware_lint_common + autoware_perception_msgs lanelet2_extension tier4_perception_msgs diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 5a68223afb5ff..c8f2ca85b2089 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -46,11 +46,68 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; // the default value is -1, which means to not set confidence - if (confidence >= 0) { + if (confidence > 0) { signal.elements[0].confidence = confidence; } } +bool hasTrafficLightCircleColor( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape( + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop( + const lanelet::ConstLanelet & lanelet, + const autoware_perception_msgs::msg::TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && + hasTrafficLightShape( + tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light) { const auto & tl_bl = traffic_light.front(); diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index e88a5ee833612..960aadf226b70 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -358,25 +358,29 @@ bool AEB::hasCollision( const double current_v, const Path & ego_path, const std::vector & objects) { // calculate RSS - const auto current_p = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + const auto current_p = tier4_autoware_utils::createPoint( + ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); const double & t = t_response_; for (const auto & obj : objects) { const double & obj_v = obj.velocity; const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; - const double dist_ego_to_object = - motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - - vehicle_info_.max_longitudinal_offset_m; - if (dist_ego_to_object < rss_dist) { - // collision happens - ObjectData collision_data = obj; - collision_data.rss = rss_dist; - collision_data.distance_to_object = dist_ego_to_object; - collision_data_keeper_.update(collision_data); - return true; + + // check the object is front the ego or not + if ((obj.position.x - ego_path[0].position.x) > 0) { + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj.position) - + vehicle_info_.max_longitudinal_offset_m; + if (dist_ego_to_object < rss_dist) { + // collision happens + ObjectData collision_data = obj; + collision_data.rss = rss_dist; + collision_data.distance_to_object = dist_ego_to_object; + collision_data_keeper_.update(collision_data); + return true; + } } } - return false; } diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp index 39ec316dbe437..8033ac9442960 100644 --- a/control/control_validator/include/control_validator/utils.hpp +++ b/control/control_validator/include/control_validator/utils.hpp @@ -15,7 +15,7 @@ #ifndef CONTROL_VALIDATOR__UTILS_HPP_ #define CONTROL_VALIDATOR__UTILS_HPP_ -#include +#include #include #include diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4518de37b7034..b8ee8ad79a33d 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -37,6 +37,7 @@ | `steering_angle_velocity` | double | steering angle velocity for operation | | `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | | `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `raw_control` | bool | skip input odometry if true | | `velocity_gain` | double | ratio to calculate velocity by acceleration | | `max_forward_velocity` | double | absolute max velocity to go forward | | `max_backward_velocity` | double | absolute max velocity to go backward | @@ -85,3 +86,25 @@ | Autoware Disengage | ○ | | Vehicle Engage | △ | | Vehicle Disengage | △ | + +## XBOX Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | RT | +| Brake | LT | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | LB | +| Turn Signal Right | RB | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | View | +| Clear Emergency Stop | Menu | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | Left Stick Button | +| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml index 8d48948a2d133..73a5d028985c5 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller.param.yaml @@ -9,6 +9,7 @@ accel_sensitivity: 1.0 brake_sensitivity: 1.0 control_command: + raw_control: false velocity_gain: 3.0 max_forward_velocity: 20.0 max_backward_velocity: 3.0 diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index c6de42afc38d9..22064f9cefaad 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node double brake_sensitivity_; // ControlCommand Parameter + bool raw_control_; double velocity_gain_; double max_forward_velocity_; double max_backward_velocity_; diff --git a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp new file mode 100644 index 0000000000000..a009ee1e12975 --- /dev/null +++ b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ + +#include "joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace joy_controller +{ +class XBOXJoyConverter : public JoyConverterBase +{ +public: + explicit XBOXJoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const { return std::max(0.0f, -((RT() - 1.0f) / 2.0f)); } + + float brake() const { return std::max(0.0f, -((LT() - 1.0f) / 2.0f)); } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return LB(); } + bool turn_signal_right() const { return RB(); } + bool clear_turn_signal() const { return A(); } + + bool gate_mode() const { return B(); } + + bool emergency_stop() const { return ChangeView(); } + bool clear_emergency_stop() const { return Menu(); } + + bool autoware_engage() const { return X(); } + bool autoware_disengage() const { return Y(); } + + bool vehicle_engage() const { return LStickButton(); } + bool vehicle_disengage() const { return RStickButton(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float RStickLeftRight() const { return j_.axes.at(2); } + float RStickUpDown() const { return j_.axes.at(3); } + float RT() const { return j_.axes.at(4); } + float LT() const { return j_.axes.at(5); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + + bool A() const { return j_.buttons.at(0); } + bool B() const { return j_.buttons.at(1); } + bool X() const { return j_.buttons.at(3); } + bool Y() const { return j_.buttons.at(4); } + bool LB() const { return j_.buttons.at(6); } + bool RB() const { return j_.buttons.at(7); } + bool Menu() const { return j_.buttons.at(11); } + bool LStickButton() const { return j_.buttons.at(13); } + bool RStickButton() const { return j_.buttons.at(14); } + bool ChangeView() const { return j_.buttons.at(15); } + bool Xbox() const { return j_.buttons.at(16); } + + const sensor_msgs::msg::Joy j_; +}; +} // namespace joy_controller + +#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index f719d8bd78cb5..d2804a7339046 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json index c67c575a6bd41..d4c6351324935 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -10,7 +10,7 @@ "type": "string", "description": "Joy controller type", "default": "DS4", - "enum": ["P65", "DS4", "G29"] + "enum": ["P65", "DS4", "G29", "XBOX"] }, "update_rate": { "type": "number", @@ -55,6 +55,11 @@ "control_command": { "type": "object", "properties": { + "raw_control": { + "type": "boolean", + "description": "Whether to skip input odometry", + "default": false + }, "velocity_gain": { "type": "number", "description": "Ratio to calculate velocity by acceleration", @@ -79,6 +84,7 @@ } }, "required": [ + "raw_control", "velocity_gain", "max_forward_velocity", "max_backward_velocity", diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 897986a7a41cf..5eec438032410 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -16,6 +16,7 @@ #include "joy_controller/joy_converter/ds4_joy_converter.hpp" #include "joy_controller/joy_converter/g29_joy_converter.hpp" #include "joy_controller/joy_converter/p65_joy_converter.hpp" +#include "joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -154,6 +155,8 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt joy_ = std::make_shared(*msg); } else if (joy_type_ == "DS4") { joy_ = std::make_shared(*msg); + } else if (joy_type_ == "XBOX") { + joy_ = std::make_shared(*msg); } else { joy_ = std::make_shared(*msg); } @@ -217,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady() } // Twist - { + if (!raw_control_) { if (!twist_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), @@ -458,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & steering_angle_velocity_ = declare_parameter("steering_angle_velocity"); accel_sensitivity_ = declare_parameter("accel_sensitivity"); brake_sensitivity_ = declare_parameter("brake_sensitivity"); + raw_control_ = declare_parameter("control_command.raw_control"); velocity_gain_ = declare_parameter("control_command.velocity_gain"); max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity"); max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity"); @@ -477,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & sub_joy_ = this->create_subscription( "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); + if (!raw_control_) { + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), + subscriber_option); + } else { + twist_ = std::make_shared(); + } // Publisher pub_control_command_ = diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/lane_departure_checker/schema/lane_departure_checker.json new file mode 100644 index 0000000000000..c7f39fbf7ef8a --- /dev/null +++ b/control/lane_departure_checker/schema/lane_departure_checker.json @@ -0,0 +1,79 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lane_departure_checker", + "type": "object", + "definitions": { + "lane_departure_checker": { + "type": "object", + "properties": { + "footprint_margin_scale": { + "type": "number", + "default": 1.0, + "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation." + }, + "resample_interval": { + "type": "number", + "default": 0.3, + "description": "Minimum Euclidean distance between points when resample trajectory.[m]." + }, + "max_deceleration": { + "type": "number", + "default": 2.8, + "description": "Maximum deceleration when calculating braking distance." + }, + "delay_time": { + "type": "number", + "default": 1.3, + "description": "Delay time which took to actuate brake when calculating braking distance. [second]." + }, + "max_lateral_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum lateral deviation in vehicle coordinate. [m]." + }, + "max_longitudinal_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum longitudinal deviation in vehicle coordinate. [m]." + }, + "max_yaw_deviation_deg": { + "type": "number", + "default": 60.0, + "description": "Maximum ego yaw deviation from trajectory. [deg]." + }, + "ego_nearest_dist_threshold": { + "type": "number" + }, + "ego_nearest_yaw_threshold": { + "type": "number" + }, + "min_braking_distance": { + "type": "number" + } + }, + "required": [ + "footprint_margin_scale", + "resample_interval", + "max_deceleration", + "max_lateral_deviation", + "max_longitudinal_deviation", + "max_yaw_deviation_deg", + "ego_nearest_dist_threshold", + "ego_nearest_yaw_threshold", + "min_braking_distance" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lane_departure_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} 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/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp index 07a6e2c6b0d21..8fbf3488c5a2e 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -120,10 +120,8 @@ bool filt_vector(const int num, std::vector & u) double tmp = 0.0; int num_tmp = 0; double count = 0; - if (i - num < 0) { - num_tmp = i; - } else if (i + num > static_cast(u.size()) - 1) { - num_tmp = static_cast(u.size()) - i - 1; + if ((i - num < 0) || (i + num > static_cast(u.size()) - 1)) { + num_tmp = std::min(i, static_cast(u.size()) - i - 1); } else { num_tmp = num; } 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_lowpass_filter.cpp b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp index 2cd641c031265..c68513586847b 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -61,6 +61,30 @@ TEST(TestLowpassFilter, MoveAverageFilter) EXPECT_EQ(filtered_vec[4], 23.0 / 3); EXPECT_EQ(filtered_vec[5], original_vec[5]); } + { + const int window_size = 3; + const std::vector original_vec = {1.0, 1.0, 1.0, 1.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 1.0); + EXPECT_EQ(filtered_vec[2], 1.0); + EXPECT_EQ(filtered_vec[3], original_vec[3]); + } + { + const int window_size = 4; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 8.0 / 3); + EXPECT_EQ(filtered_vec[2], 21.0 / 5); + EXPECT_EQ(filtered_vec[3], 30.0 / 5); + EXPECT_EQ(filtered_vec[4], 23.0 / 3); + EXPECT_EQ(filtered_vec[5], original_vec[5]); + } } TEST(TestLowpassFilter, Butterworth2dFilter) { 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/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index a110cb94d1d91..57eab2d87f18e 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -241,6 +241,10 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig ### STOPPED Parameter +The `STOPPED` state assumes that the vehicle is completely stopped with the brakes fully applied. +Therefore, `stopped_acc` should be set to a value that allows the vehicle to apply the strongest possible brake. +If `stopped_acc` is not sufficiently low, there is a possibility of sliding down on steep slopes. + | Name | Type | Description | Default value | | :----------- | :----- | :------------------------------------------- | :------------ | | stopped_vel | double | target velocity in STOPPED state [m/s] | 0.0 | diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 9c01f7bc26c4b..cc1c9c08887ba 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -16,6 +16,8 @@ #define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" @@ -74,14 +76,6 @@ Pose calcPoseAfterTimeDelay( const Pose & current_pose, const double delay_time, const double current_vel, const double current_acc); -/** - * @brief apply linear interpolation to orientation - * @param [in] o_from first orientation - * @param [in] o_to second orientation - * @param [in] ratio ratio between o_from and o_to for interpolation - */ -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - /** * @brief apply linear interpolation to trajectory point that is nearest to a certain point * @param [in] points trajectory points @@ -107,7 +101,7 @@ TrajectoryPoint lerpTrajectoryPoint( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); interpolated_point.pose.position.y = interpolation::lerp( points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio); - interpolated_point.pose.orientation = lerpOrientation( + interpolated_point.pose.orientation = interpolation::lerpOrientation( points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); interpolated_point.longitudinal_velocity_mps = interpolation::lerp( points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 9791a1f0b5e3e..33a54663ccaef 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio) return v_from + (v_to - v_from) * ratio; } -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - double applyDiffLimitFilter( const double input_val, const double prev_val, const double dt, const double max_val, const double min_val) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 12f0eece1e477..31c48fb5060a8 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -555,9 +555,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return; }; - const auto info_throttle = [this](const auto & s) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s); - }; + const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%s", s); }; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; @@ -623,10 +621,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_control_state == ControlState::STOPPED) { // -- debug print -- if (has_nonzero_target_vel && !departure_condition_from_stopped) { - info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED."); + info_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } if (has_nonzero_target_vel && keep_stopped_condition) { - info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED."); + info_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); } // --------------- @@ -646,10 +644,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in EMERGENCY state if (m_control_state == ControlState::EMERGENCY) { + if (stopped_condition) { + return changeState(ControlState::STOPPED); + } + if (!emergency_condition) { - if (stopped_condition) { - return changeState(ControlState::STOPPED); - } if (!is_under_control) { // NOTE: On manual driving, no need stopping to exit the emergency. return changeState(ControlState::DRIVE); diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 5c7698180f82b..58aa867b2deab 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "gtest/gtest.h" +#include "interpolation/spherical_linear_interpolation.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" @@ -303,7 +305,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) o_to.setRPY(M_PI_4, M_PI_4, M_PI_4); ratio = 0.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -311,7 +313,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); ratio = 1.0; - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4); @@ -320,7 +322,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) ratio = 0.5; o_to.setRPY(M_PI_4, 0.0, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2); @@ -328,7 +330,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, M_PI_4, 0.0); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); @@ -336,7 +338,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) EXPECT_DOUBLE_EQ(yaw, 0.0); o_to.setRPY(0.0, 0.0, M_PI_4); - result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); + result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio); tf2::convert(result, o_result); tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw); EXPECT_DOUBLE_EQ(roll, 0.0); diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index d21b11e5b68de..1d7791955b576 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,8 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include #include -#include #include #include #include diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index e3c3b5cccfb8f..3ce5728521141 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index daaebaacde8de..ca0d77140b195 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -38,7 +38,7 @@ #include "trajectory_follower_base/lateral_controller_base.hpp" #include -#include +#include #include #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" 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/evaluator/diagnostic_converter/include/converter_node.hpp b/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp similarity index 93% rename from evaluator/diagnostic_converter/include/converter_node.hpp rename to evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp index 53c762dac0ffe..59bb02ebf301f 100644 --- a/evaluator/diagnostic_converter/include/converter_node.hpp +++ b/evaluator/diagnostic_converter/include/diagnostic_converter/converter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONVERTER_NODE_HPP_ -#define CONVERTER_NODE_HPP_ +#ifndef DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ +#define DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ #include @@ -63,4 +63,4 @@ class DiagnosticConverter : public rclcpp::Node }; } // namespace diagnostic_converter -#endif // CONVERTER_NODE_HPP_ +#endif // DIAGNOSTIC_CONVERTER__CONVERTER_NODE_HPP_ diff --git a/evaluator/diagnostic_converter/src/converter_node.cpp b/evaluator/diagnostic_converter/src/converter_node.cpp index e61b19833d2fe..2a2574732694c 100644 --- a/evaluator/diagnostic_converter/src/converter_node.cpp +++ b/evaluator/diagnostic_converter/src/converter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "converter_node.hpp" +#include "diagnostic_converter/converter_node.hpp" #include diff --git a/evaluator/diagnostic_converter/test/test_converter_node.cpp b/evaluator/diagnostic_converter/test/test_converter_node.cpp index 47f42d018ea71..167421f0777df 100644 --- a/evaluator/diagnostic_converter/test/test_converter_node.cpp +++ b/evaluator/diagnostic_converter/test/test_converter_node.cpp @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "converter_node.hpp" -#include "gtest/gtest.h" +#include "diagnostic_converter/converter_node.hpp" #include @@ -21,6 +20,8 @@ #include "tier4_simulation_msgs/msg/user_defined_value.hpp" #include "tier4_simulation_msgs/msg/user_defined_value_type.hpp" +#include + #include #include #include diff --git a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp index d0d8d61c4c231..53265872bffa8 100644 --- a/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -125,7 +125,7 @@ Stat calcTrajectoryLength(const Trajectory & traj) Stat calcTrajectoryDuration(const Trajectory & traj) { double duration = 0.0; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double length = calcDistance2d(traj.points.at(i), traj.points.at(i + 1)); const double velocity = traj.points.at(i).longitudinal_velocity_mps; if (velocity != 0) { @@ -158,7 +158,7 @@ Stat calcTrajectoryAcceleration(const Trajectory & traj) Stat calcTrajectoryJerk(const Trajectory & traj) { Stat stat; - for (size_t i = 0; i < traj.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double vel = traj.points.at(i).longitudinal_velocity_mps; if (vel != 0) { const double duration = diff --git a/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 8538179253480..f4d6679291849 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -14,15 +14,16 @@ - - - + + - + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 181f470a00800..ac7589ea2273b 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -1,17 +1,13 @@ - - - - - - - - - - - - + + + + + + + + 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_map_launch/package.xml b/launch/tier4_map_launch/package.xml index d8f69c124526a..f75a181bfb659 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -7,6 +7,7 @@ Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda + Kento Yabuuchi Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml deleted file mode 100644 index 65caad54876ff..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ /dev/null @@ -1,435 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 0d03e3c8cdcd9..ab9ed65999048 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -3,12 +3,12 @@ - + - + @@ -34,14 +34,10 @@ - - - - - - + - + + @@ -62,23 +58,61 @@ - - - - - + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + @@ -99,45 +133,107 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + + + + + + + + + + + + + + - - - - - + + + + + + + + + + + + + + + - + - + + + + + - - - + + + + + + + + - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml similarity index 55% rename from launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 227aac50d6d90..1c9201a9af8b3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -1,20 +1,13 @@ - - - - + - - - - + - - - - + + + @@ -57,6 +50,44 @@ --> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -65,7 +96,7 @@ - + @@ -113,6 +144,7 @@ + @@ -167,9 +199,7 @@ - - - + @@ -196,187 +226,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml new file mode 100644 index 0000000000000..1a97659b357d8 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml new file mode 100644 index 0000000000000..ce34640bd3179 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml similarity index 100% rename from launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml deleted file mode 100644 index ed37f6270c913..0000000000000 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml new file mode 100644 index 0000000000000..2f62e83ae0ef5 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml new file mode 100644 index 0000000000000..b4d19c9052a63 --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -0,0 +1,154 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml similarity index 51% rename from launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml rename to launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index 2c8b7d6581c53..6ccff44933966 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -1,113 +1,19 @@ - - + - - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - + + + @@ -133,9 +39,9 @@ - - - + + + diff --git a/launch/tier4_perception_launch/launch/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 12190fb659235..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 @@ -78,7 +78,7 @@ def create_additional_pipeline(self, lidar_name): plugin="pointcloud_preprocessor::CropBoxFilterComponent", name=f"{lidar_name}_crop_box_filter", remappings=[ - ("input", f"/sensing/lidar/{lidar_name}/outlier_filtered/pointcloud"), + ("input", f"/sensing/lidar/{lidar_name}/pointcloud"), ("output", f"{lidar_name}/range_cropped/pointcloud"), ], parameters=[ @@ -505,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"), @@ -515,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] @@ -531,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 751eeea66c7b6..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,8 +7,8 @@ - - + + @@ -24,7 +24,7 @@ - + @@ -40,7 +40,7 @@ - + @@ -48,4 +48,18 @@ + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index da393b1efff59..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 @@ + + + @@ -86,18 +92,18 @@ default="$(var data_path)/traffic_light_fine_detector" description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6" /> - + - - - - - + @@ -121,7 +127,7 @@ - + @@ -136,7 +142,7 @@ - + @@ -172,21 +178,22 @@ + + + + - - - - + @@ -230,6 +237,12 @@ + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 58e9e231e4aa0..7b81a42fdb79f 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -7,8 +7,10 @@ - - + + + + @@ -24,8 +26,10 @@ - - + + + + @@ -34,6 +38,8 @@ + + @@ -54,15 +60,18 @@ - - - + + + + + + @@ -72,11 +81,13 @@ - + + + @@ -84,6 +95,8 @@ + + @@ -104,15 +117,18 @@ - - - + + + + + + @@ -122,7 +138,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index a6bcb40e81252..dc5a2443d3f45 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -42,7 +42,15 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") add_launch_arg( - "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals" + "output/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", + ) + add_launch_arg( + "output/car/traffic_signals", "/perception/traffic_light_recognition/car/traffic_signals" + ) + add_launch_arg( + "output/pedestrian/traffic_signals", + "/perception/traffic_light_recognition/pedestrian/traffic_signals", ) # traffic_light_fine_detector @@ -63,15 +71,26 @@ def add_launch_arg(name: str, default_value=None, description=None): # traffic_light_classifier add_launch_arg("classifier_type", "1") add_launch_arg( - "classifier_model_path", + "car_classifier_model_path", os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"), ) add_launch_arg( - "classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") + "pedestrian_classifier_model_path", + os.path.join( + classifier_share_dir, "data", "pedestrian_traffic_light_classifier_efficientNet_b1.onnx" + ), + ) + add_launch_arg( + "car_classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") + ) + add_launch_arg( + "pedestrian_classifier_label_path", + os.path.join(classifier_share_dir, "data", "lamp_labels_ped.txt"), ) add_launch_arg("classifier_precision", "fp16") add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]") + add_launch_arg("backlight_threshold", "0.85") add_launch_arg("use_intra_process", "False") add_launch_arg("use_multithread", "False") @@ -91,23 +110,56 @@ def create_parameter_dict(*args): ComposableNode( package="traffic_light_classifier", plugin="traffic_light::TrafficLightClassifierNodelet", - name="traffic_light_classifier", + name="car_traffic_light_classifier", + namespace="classification", + parameters=[ + { + "approximate_sync": LaunchConfiguration("approximate_sync"), + "classifier_type": LaunchConfiguration("classifier_type"), + "classify_traffic_light_type": 0, + "classifier_model_path": LaunchConfiguration("car_classifier_model_path"), + "classifier_label_path": LaunchConfiguration("car_classifier_label_path"), + "classifier_precision": LaunchConfiguration("classifier_precision"), + "classifier_mean": LaunchConfiguration("classifier_mean"), + "classifier_std": LaunchConfiguration("classifier_std"), + "backlight_threshold": LaunchConfiguration("backlight_threshold"), + } + ], + remappings=[ + ("~/input/image", LaunchConfiguration("input/image")), + ("~/input/rois", LaunchConfiguration("output/rois")), + ("~/output/traffic_signals", "classified/car/traffic_signals"), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="traffic_light_classifier", + plugin="traffic_light::TrafficLightClassifierNodelet", + name="pedestrian_traffic_light_classifier", namespace="classification", parameters=[ - create_parameter_dict( - "approximate_sync", - "classifier_type", - "classifier_model_path", - "classifier_label_path", - "classifier_precision", - "classifier_mean", - "classifier_std", - ) + { + "approximate_sync": LaunchConfiguration("approximate_sync"), + "classifier_type": LaunchConfiguration("classifier_type"), + "classify_traffic_light_type": 1, + "classifier_model_path": LaunchConfiguration( + "pedestrian_classifier_model_path" + ), + "classifier_label_path": LaunchConfiguration( + "pedestrian_classifier_label_path" + ), + "classifier_precision": LaunchConfiguration("classifier_precision"), + "classifier_mean": LaunchConfiguration("classifier_mean"), + "classifier_std": LaunchConfiguration("classifier_std"), + "backlight_threshold": LaunchConfiguration("backlight_threshold"), + } ], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), - ("~/output/traffic_signals", "classified/traffic_signals"), + ("~/output/traffic_signals", "classified/pedestrian/traffic_signals"), ], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} @@ -122,7 +174,10 @@ def create_parameter_dict(*args): ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), ("~/input/rough/rois", "detection/rough/rois"), - ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")), + ( + "~/input/traffic_signals", + LaunchConfiguration("output/traffic_signals"), + ), ("~/output/image", "debug/rois"), ("~/output/image/compressed", "debug/rois/compressed"), ("~/output/image/compressedDepth", "debug/rois/compressedDepth"), 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 3a1f417106f54..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 @@ + @@ -16,17 +17,6 @@ - - - - - - - - - - - @@ -42,6 +32,9 @@ + + + @@ -95,7 +88,7 @@ value="$(eval "'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::AvoidanceByLaneChangeModuleManager, '")" if="$(var launch_avoidance_by_lane_change_module)" /> - + @@ -174,7 +167,12 @@ value="$(eval "'$(var behavior_velocity_planner_launch_modules)' + 'behavior_velocity_planner::NoDrivableLaneModulePlugin, '")" if="$(var launch_no_drivable_lane_module)" /> - + + @@ -198,6 +196,7 @@ + @@ -208,7 +207,6 @@ - @@ -239,6 +237,7 @@ + @@ -254,6 +253,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 6492f20331a66..02f14a86cd7df 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -32,71 +32,33 @@ This package includes the following features:

-## Launch - -The `ekf_localizer` starts with the default parameters with the following command. - -```sh -roslaunch ekf_localizer ekf_localizer.launch -``` - -The parameters and input topic names can be set in the `ekf_localizer.launch` file. - ## Node ### Subscribed Topics -- measured_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Input pose source with the measurement covariance matrix. - -- measured_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - Input twist source with the measurement covariance matrix. - -- initialpose (geometry_msgs/PoseWithCovarianceStamped) - - Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. +| Name | Type | Description | +| -------------------------------- | ------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Input pose source with the measurement covariance matrix. | +| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Input twist source with the measurement covariance matrix. | +| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Initial pose for EKF. The estimated pose is initialized with zeros at the start. It is initialized with this message whenever published. | ### Published Topics -- ekf_odom (nav_msgs/Odometry) - - Estimated odometry. - -- ekf_pose (geometry_msgs/PoseStamped) - - Estimated pose. - -- ekf_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance. - -- ekf_biased_pose (geometry_msgs/PoseStamped) - - Estimated pose including the yaw bias - -- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped) - - Estimated pose with covariance including the yaw bias - -- ekf_twist (geometry_msgs/TwistStamped) - - Estimated twist. - -- ekf_twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) - - The estimated twist with covariance. - -- diagnostics (diagnostic_msgs/DiagnosticArray) - - The diagnostic information. +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | ### Published TF - base_link - - TF from "map" coordinate to estimated pose. + TF from `map` coordinate to estimated pose. ## Functions @@ -121,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 | @@ -152,6 +115,14 @@ The parameters are set in `launch/ekf_localizer.launch` . note: process noise for positions x & y are calculated automatically from nonlinear dynamics. +### Simple 1D Filter Parameters + +| Name | Type | Description | Default value | +| :-------------------- | :----- | :---------------------------------------------- | :------------ | +| z_filter_proc_dev | double | Simple1DFilter - Z filter process deviation | 1.0 | +| roll_filter_proc_dev | double | Simple1DFilter - Roll filter process deviation | 0.01 | +| pitch_filter_proc_dev | double | Simple1DFilter - Pitch filter process deviation | 0.01 | + ### For diagnostics | Name | Type | Description | Default value | @@ -197,7 +168,9 @@ Increasing the number will improve the smoothness of the estimation, but may hav -where `b_k` is the yawbias. +where, $\theta_k$ represents the vehicle's heading angle, including the mounting angle bias. +$b_k$ is a correction term for the yaw bias, and it is modeled so that $(\theta_k+b_k)$ becomes the heading angle of the base_link. +The pose_estimator is expected to publish the base_link in the map coordinate system. However, the yaw angle may be offset due to calibration errors. This model compensates this error and improves estimation accuracy. ### time delay model @@ -232,7 +205,7 @@ Note that, although the dimension gets larger since the analytical expansion can ## Known issues -- In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. +- If multiple pose_estimators are used, the input to the EKF will include multiple yaw biases corresponding to each source. However, the current EKF assumes the existence of only one yaw bias. Therefore, yaw bias `b_k` in the current EKF state would not make any sense and cannot correctly handle these multiple yaw biases. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. ## reference diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 1c16624605907..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 @@ -22,6 +23,11 @@ proc_stddev_vx_c: 10.0 proc_stddev_wz_c: 5.0 + #Simple1DFilter parameters + z_filter_proc_dev: 1.0 + roll_filter_proc_dev: 0.01 + pitch_filter_proc_dev: 0.01 + # for diagnostics pose_no_update_count_threshold_warn: 50 pose_no_update_count_threshold_error: 100 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 01ef658cf445d..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"))), @@ -40,6 +41,9 @@ class HyperParameters proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)), proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)), proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)), + z_filter_proc_dev(node->declare_parameter("z_filter_proc_dev", 1.0)), + roll_filter_proc_dev(node->declare_parameter("roll_filter_proc_dev", 0.01)), + pitch_filter_proc_dev(node->declare_parameter("pitch_filter_proc_dev", 0.01)), pose_no_update_count_threshold_warn( node->declare_parameter("pose_no_update_count_threshold_warn", 50)), pose_no_update_count_threshold_error( @@ -57,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; @@ -69,6 +74,9 @@ class HyperParameters const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise + const double z_filter_proc_dev; + const double roll_filter_proc_dev; + const double pitch_filter_proc_dev; const size_t pose_no_update_count_threshold_warn; const size_t pose_no_update_count_threshold_error; const size_t twist_no_update_count_threshold_warn; 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 a3d2f52a4058c..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_ = @@ -94,22 +96,22 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti ekf_module_ = std::make_unique(warning_, params_); logger_configure_ = std::make_unique(this); - z_filter_.set_proc_dev(1.0); - roll_filter_.set_proc_dev(0.01); - pitch_filter_.set_proc_dev(0.01); + z_filter_.set_proc_dev(params_.z_filter_proc_dev); + roll_filter_.set_proc_dev(params_.roll_filter_proc_dev); + pitch_filter_.set_proc_dev(params_.pitch_filter_proc_dev); } /* * 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/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 10926b04507a0..8977d82f34138 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -87,9 +87,15 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( { const double x = kalman_filter_.getXelement(IDX::X); const double y = kalman_filter_.getXelement(IDX::Y); + /* + getXelement(IDX::YAW) is surely `biased_yaw`. + Please note how `yaw` and `yaw_bias` are used in the state transition model and + how the observed pose is handled in the measurement pose update. + */ const double biased_yaw = kalman_filter_.getXelement(IDX::YAW); const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB); const double yaw = biased_yaw + yaw_bias; + Pose current_ekf_pose; current_ekf_pose.header.frame_id = params_.pose_frame_id; current_ekf_pose.header.stamp = current_time; @@ -167,10 +173,10 @@ void EKFModule::accumulate_delay_time(const double dt) accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1, accumulated_delay_times_.end()); - // Add the new delay time to all elements. + // Add a new element (=0) and, and add delay time to the previous elements. accumulated_delay_times_.front() = 0.0; - for (auto & delay_time : accumulated_delay_times_) { - delay_time += dt; + for (size_t i = 1; i < accumulated_delay_times_.size(); ++i) { + accumulated_delay_times_[i] += dt; } } @@ -224,7 +230,9 @@ bool EKFModule::measurementUpdatePose( return false; } - /* Set yaw */ + /* Since the kalman filter cannot handle the rotation angle directly, + offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less + than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi 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/README.md b/localization/landmark_based_localizer/README.md index 86f3750ad11d9..49588a19ac620 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -43,68 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret. - -The four vertices of a landmark are defined counterclockwise. - -The order of the four vertices is defined as follows. In the coordinate system of a landmark, - -- the x-axis is parallel to the vector from the first vertex to the second vertex -- the y-axis is parallel to the vector from the second vertex to the third vertex - -![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) - -### Example of `lanelet2_map.osm` - -The values provided below are placeholders. -Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. - -For example, when using the AR tag, it would look like this. - -```xml -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` +See ## About `consider_orientation` diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index f0c54d6393f7e..1a776bd810fff 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -2,7 +2,7 @@ **ArTagBasedLocalizer** is a vision-based localization node. - +ar_tag_image This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. @@ -30,6 +30,10 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | +## Parameters + +{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} + ## How to launch When launching Autoware, set `artag` for `pose_source`. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 6437455875cc2..272338905c3f0 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -8,18 +8,24 @@ - + + + + - + + + + 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 7e83220dadf2a..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 @@ -17,7 +20,6 @@ aruco cv_bridge diagnostic_msgs - image_transport landmark_manager lanelet2_extension localization_util diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json new file mode 100644 index 0000000000000..bde5221aa1bbc --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json @@ -0,0 +1,87 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ar_tag_based_localizer", + "type": "object", + "definitions": { + "ar_tag_based_localizer": { + "type": "object", + "properties": { + "marker_size": { + "type": "number", + "description": "marker_size", + "default": 0.6 + }, + "target_tag_ids": { + "type": "array", + "description": "target_tag_ids", + "default": "['0','1','2','3','4','5','6']" + }, + "base_covariance": { + "type": "array", + "description": "base_covariance", + "default": [ + 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.02 + ] + }, + "distance_threshold": { + "type": "number", + "description": "distance_threshold(m)", + "default": "13.0" + }, + "consider_orientation": { + "type": "boolean", + "description": "consider_orientation", + "default": "false" + }, + "detection_mode": { + "type": "string", + "description": "detection_mode select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST]", + "default": "DM_NORMAL" + }, + "min_marker_size": { + "type": "number", + "description": "min_marker_size", + "default": 0.02 + }, + "ekf_time_tolerance": { + "type": "number", + "description": "ekf_time_tolerance(sec)", + "default": 5.0 + }, + "ekf_position_tolerance": { + "type": "number", + "description": "ekf_position_tolerance(m)", + "default": 10.0 + } + }, + "required": [ + "marker_size", + "target_tag_ids", + "base_covariance", + "distance_threshold", + "consider_orientation", + "detection_mode", + "min_marker_size", + "ekf_time_tolerance", + "ekf_position_tolerance" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ar_tag_based_localizer" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index e569a71bbb5b0..43ac1e1098453 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -66,10 +66,6 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : Node("ar_tag_based_localizer", options), cam_info_received_(false) -{ -} - -bool ArTagBasedLocalizer::setup() { /* Declare node parameters @@ -92,7 +88,7 @@ bool ArTagBasedLocalizer::setup() } else { // Error RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); - return false; + return; } ekf_pose_buffer_ = std::make_unique( this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); @@ -111,59 +107,50 @@ bool ArTagBasedLocalizer::setup() tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); - /* - Initialize image transport - */ - it_ = std::make_unique(shared_from_this()); - /* Subscribers */ + using std::placeholders::_1; map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1)); + std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); - pose_array_pub_ = this->create_publisher("~/debug/detected_landmarks", qos_sub); image_sub_ = this->create_subscription( - "~/input/image", qos_sub, - std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); + "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, _1)); cam_info_sub_ = this->create_subscription( - "~/input/camera_info", qos_sub, - std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, _1)); ekf_pose_sub_ = this->create_subscription( - "~/input/ekf_pose", qos_sub, - std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); + "~/input/ekf_pose", qos_sub, std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, _1)); /* Publishers */ - rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); - qos_marker.transient_local(); - qos_marker.reliable(); - marker_pub_ = this->create_publisher("~/debug/marker", qos_marker); - rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - image_pub_ = it_->advertise("~/debug/result", 1); - pose_pub_ = - this->create_publisher("~/output/pose_with_covariance", qos_pub); - diag_pub_ = this->create_publisher("/diagnostics", qos_pub); + const rclcpp::QoS qos_pub_once = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local().reliable(); + const rclcpp::QoS qos_pub_periodic(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + pose_pub_ = this->create_publisher( + "~/output/pose_with_covariance", qos_pub_periodic); + image_pub_ = this->create_publisher("~/debug/image", qos_pub_periodic); + mapped_tag_pose_pub_ = this->create_publisher("~/debug/mapped_tag", qos_pub_once); + detected_tag_pose_pub_ = + this->create_publisher("~/debug/detected_tag", qos_pub_periodic); + diag_pub_ = this->create_publisher("/diagnostics", qos_pub_periodic); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); - return true; } void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { - landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger()); + landmark_manager_.parse_landmarks(msg, "apriltag_16h5"); const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); - marker_pub_->publish(marker_msg); + mapped_tag_pose_pub_->publish(marker_msg); } void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { // check subscribers - if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { + if ((image_pub_->get_subscription_count() == 0) && (pose_pub_->get_subscription_count() == 0)) { RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); return; } @@ -186,22 +173,22 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; // detect - const std::vector landmarks = detect_landmarks(msg); + const std::vector landmarks = detect_landmarks(msg); if (landmarks.empty()) { return; } // for debug - if (pose_array_pub_->get_subscription_count() > 0) { + if (detected_tag_pose_pub_->get_subscription_count() > 0) { PoseArray pose_array_msg; pose_array_msg.header.stamp = sensor_stamp; pose_array_msg.header.frame_id = "map"; - for (const landmark_manager::Landmark & landmark : landmarks) { + for (const Landmark & landmark : landmarks) { const Pose detected_marker_on_map = tier4_autoware_utils::transformPose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); } - pose_array_pub_->publish(pose_array_msg); + detected_tag_pose_pub_->publish(pose_array_msg); } // calc new_self_pose @@ -267,24 +254,12 @@ void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & m return; } - cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - camera_matrix.at(0, 0) = msg->p[0]; - camera_matrix.at(0, 1) = msg->p[1]; - camera_matrix.at(0, 2) = msg->p[2]; - camera_matrix.at(0, 3) = msg->p[3]; - camera_matrix.at(1, 0) = msg->p[4]; - camera_matrix.at(1, 1) = msg->p[5]; - camera_matrix.at(1, 2) = msg->p[6]; - camera_matrix.at(1, 3) = msg->p[7]; - camera_matrix.at(2, 0) = msg->p[8]; - camera_matrix.at(2, 1) = msg->p[9]; - camera_matrix.at(2, 2) = msg->p[10]; - camera_matrix.at(2, 3) = msg->p[11]; - - cv::Mat distortion_coeff(4, 1, CV_64FC1); - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } + // copy camera matrix + cv::Mat camera_matrix(3, 4, CV_64FC1); + std::copy(msg->p.begin(), msg->p.end(), camera_matrix.begin()); + + // all 0 + cv::Mat distortion_coeff(4, 1, CV_64FC1, 0.0); const cv::Size size(static_cast(msg->width), static_cast(msg->height)); @@ -318,7 +293,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( cv_ptr->image.copyTo(in_image); } catch (cv_bridge::Exception & e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return std::vector{}; + return std::vector{}; } // get transform from base_link to camera @@ -328,7 +303,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); - return std::vector{}; + return std::vector{}; } // detect @@ -336,7 +311,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( detector_.detect(in_image, markers, cam_param_, marker_size_, false); // parse - std::vector landmarks; + std::vector landmarks; for (aruco::Marker & marker : markers) { // convert marker pose to tf @@ -352,7 +327,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z); if (distance <= distance_threshold_) { tf2::doTransform(pose, pose, transform_sensor_to_base_link); - landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose}); + landmarks.push_back(Landmark{std::to_string(marker.id), pose}); } // for debug, drawing the detected markers @@ -361,12 +336,12 @@ std::vector ArTagBasedLocalizer::detect_landmarks( } // for debug - if (image_pub_.getNumSubscribers() > 0) { + if (image_pub_->get_subscription_count() > 0) { cv_bridge::CvImage out_msg; out_msg.header.stamp = sensor_stamp; out_msg.encoding = sensor_msgs::image_encodings::RGB8; out_msg.image = in_image; - image_pub_.publish(out_msg.toImageMsg()); + image_pub_->publish(*out_msg.toImageMsg()); } return landmarks; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index b7dfb45a26ce3..f70821a39ffe8 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -48,11 +48,12 @@ #include "landmark_manager/landmark_manager.hpp" #include "localization_util/smart_pose_buffer.hpp" -#include #include #include #include +#include +#include #include #include @@ -79,17 +80,17 @@ class ArTagBasedLocalizer : public rclcpp::Node using TransformStamped = geometry_msgs::msg::TransformStamped; using MarkerArray = visualization_msgs::msg::MarkerArray; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using Landmark = landmark_manager::Landmark; public: explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - bool setup(); private: void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); - std::vector detect_landmarks(const Image::ConstSharedPtr & msg); + std::vector detect_landmarks(const Image::ConstSharedPtr & msg); // Parameters float marker_size_{}; @@ -104,9 +105,6 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - // image transport - std::unique_ptr it_; - // Subscribers rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; @@ -114,10 +112,10 @@ class ArTagBasedLocalizer : public rclcpp::Node rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers - rclcpp::Publisher::SharedPtr marker_pub_; - rclcpp::Publisher::SharedPtr pose_array_pub_; - image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr detected_tag_pose_pub_; + rclcpp::Publisher::SharedPtr mapped_tag_pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; // Others diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp index cbcd57b4b222a..8ef1dd6195580 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp @@ -48,7 +48,6 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); std::shared_ptr ptr = std::make_shared(); - ptr->setup(); rclcpp::spin(ptr); rclcpp::shutdown(); } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp index 1b44327fd9e8b..5d05dd7e3755a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -55,7 +55,8 @@ class TestArTagBasedLocalizer : public ::testing::Test TEST_F(TestArTagBasedLocalizer, test_setup) // NOLINT { - EXPECT_TRUE(node_->setup()); + // Check if the constructor finishes successfully + EXPECT_TRUE(true); } int main(int argc, char ** argv) diff --git a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt index 1b6126c892d2e..1b57e5cc89018 100644 --- a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt @@ -12,7 +12,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(landmark_manager +ament_auto_add_library(landmark_manager SHARED src/landmark_manager.cpp ) 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 fba419f746b5e..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 @@ -15,6 +15,8 @@ #ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#include "lanelet2_extension/localization/landmark.hpp" + #include #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" @@ -40,13 +42,15 @@ class LandmarkManager public: void parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger); + 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( - const std::vector & detected_landmarks, - const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const; + const std::vector & detected_landmarks, const geometry_msgs::msg::Pose & self_pose, + const bool consider_orientation) const; private: // To allow multiple landmarks with the same id to be registered on a vector_map, 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 875f04edd8c47..57bfcde461af6 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -29,32 +29,17 @@ namespace landmark_manager void LandmarkManager::parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger) + const std::string & target_subtype) { - RCLCPP_INFO_STREAM(logger, "msg->format_version: " << msg->format_version); - RCLCPP_INFO_STREAM(logger, "msg->map_format: " << msg->map_format); - RCLCPP_INFO_STREAM(logger, "msg->map_version: " << msg->map_version); - RCLCPP_INFO_STREAM(logger, "msg->data.size(): " << msg->data.size()); - lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - - for (const auto & poly : lanelet_map_ptr->polygonLayer) { - const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; - if (type != "pose_marker") { - continue; - } - const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")}; - if (subtype != target_subtype) { - continue; - } - + std::vector landmarks = + lanelet::localization::parseLandmarkPolygons(msg, target_subtype); + for (const lanelet::Polygon3d & poly : landmarks) { // Get landmark_id const std::string landmark_id = poly.attributeOr("marker_id", "none"); // Get 4 vertices const auto & vertices = poly.basicPolygon(); if (vertices.size() != 4) { - RCLCPP_WARN_STREAM(logger, "vertices.size() (" << vertices.size() << ") != 4"); continue; } @@ -65,12 +50,8 @@ void LandmarkManager::parse_landmarks( const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - RCLCPP_INFO_STREAM(logger, "volume = " << volume); const double volume_threshold = 1e-5; if (volume > volume_threshold) { - RCLCPP_WARN_STREAM( - logger, - "volume (" << volume << ") > threshold (" << volume_threshold << "), This is not plane."); continue; } @@ -99,16 +80,25 @@ void LandmarkManager::parse_landmarks( // Add landmarks_map_[landmark_id].push_back(pose); - RCLCPP_INFO_STREAM(logger, "id: " << landmark_id); - RCLCPP_INFO_STREAM( - logger, - "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); - RCLCPP_INFO_STREAM( - logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " - << pose.orientation.z << ", " << pose.orientation.w); } } +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/README.md b/localization/localization_error_monitor/README.md index 2ddf41e4eac70..7912ad843ef19 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -29,10 +29,4 @@ The package monitors the following two values: ## Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | -------------------------------------------------------------------------------------------- | -| `scale` | double | scale factor for monitored values (default: 3.0) | -| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | -| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | -| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | -| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.25) | +{{ json_to_markdown("localization/localization_error_monitor/schema/localization_error_monitor.schema.json") }} diff --git a/localization/localization_error_monitor/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_error_monitor/schema/localization_error_monitor.schema.json b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json new file mode 100644 index 0000000000000..0cdd5fb946756 --- /dev/null +++ b/localization/localization_error_monitor/schema/localization_error_monitor.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Localization Error Monitor node", + "type": "object", + "definitions": { + "localization_error_monitor": { + "type": "object", + "properties": { + "scale": { + "type": "number", + "default": 3.0, + "description": "scale factor for monitored values" + }, + "error_ellipse_size": { + "type": "number", + "default": 1.5, + "description": "error threshold for long radius of confidence ellipse [m]" + }, + "warn_ellipse_size": { + "type": "number", + "default": 1.2, + "description": "warning threshold for long radius of confidence ellipse [m]" + }, + "error_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.3, + "description": "error threshold for size of confidence ellipse along lateral direction [m]" + }, + "warn_ellipse_size_lateral_direction": { + "type": "number", + "default": 0.25, + "description": "warning threshold for size of confidence ellipse along lateral direction [m]" + } + }, + "required": [ + "scale", + "error_ellipse_size", + "warn_ellipse_size", + "error_ellipse_size_lateral_direction", + "warn_ellipse_size_lateral_direction" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/localization_error_monitor" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index ade446020d101..fe65077d89649 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,7 +6,6 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp - src/tf2_listener_module.cpp src/smart_pose_buffer.cpp ) diff --git a/localization/localization_util/include/localization_util/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp deleted file mode 100644 index b332f9effe0e0..0000000000000 --- a/localization/localization_util/include/localization_util/tf2_listener_module.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ -#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ - -#include - -#include -#include -#include - -#include - -class Tf2ListenerModule -{ - using TransformStamped = geometry_msgs::msg::TransformStamped; - -public: - explicit Tf2ListenerModule(rclcpp::Node * node); - bool get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, - const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr) const; - -private: - rclcpp::Logger logger_; - tf2_ros::Buffer tf2_buffer_; - tf2_ros::TransformListener tf2_listener_; -}; - -#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ 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/localization_util/src/smart_pose_buffer.cpp b/localization/localization_util/src/smart_pose_buffer.cpp index ba293109dcc4d..bc62bfa690946 100644 --- a/localization/localization_util/src/smart_pose_buffer.cpp +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -39,10 +39,16 @@ std::optional SmartPoseBuffer::interpolate( const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; - if (target_ros_time < time_first || time_last < target_ros_time) { + if (target_ros_time < time_first) { return std::nullopt; } + // [time_last < target_ros_time] is acceptable here. + // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest + // timestamp of buffered pose (often EKF). + // However, if the timestamp difference is too large, + // it will later be rejected by validate_time_stamp_difference. + // get the nearest poses result.old_pose = *pose_buffer_.front(); for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { diff --git a/localization/localization_util/src/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp deleted file mode 100644 index 8a19ceec9f30d..0000000000000 --- a/localization/localization_util/src/tf2_listener_module.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "localization_util/tf2_listener_module.hpp" - -#include - -geometry_msgs::msg::TransformStamped identity_transform_stamped( - const builtin_interfaces::msg::Time & timestamp, const std::string & header_frame_id, - const std::string & child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = timestamp; - transform.header.frame_id = header_frame_id; - transform.child_frame_id = child_frame_id; - transform.transform.rotation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); - transform.transform.translation = tier4_autoware_utils::createTranslation(0.0, 0.0, 0.0); - return transform; -} - -Tf2ListenerModule::Tf2ListenerModule(rclcpp::Node * node) -: logger_(node->get_logger()), tf2_buffer_(node->get_clock()), tf2_listener_(tf2_buffer_) -{ -} - -bool Tf2ListenerModule::get_transform( - const builtin_interfaces::msg::Time & timestamp, const std::string & target_frame, - const std::string & source_frame, const TransformStamped::SharedPtr & transform_stamped_ptr) const -{ - const TransformStamped identity = - identity_transform_stamped(timestamp, target_frame, source_frame); - - if (target_frame == source_frame) { - *transform_stamped_ptr = identity; - return true; - } - - try { - *transform_stamped_ptr = - tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(logger_, "%s", ex.what()); - RCLCPP_ERROR(logger_, "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); - - *transform_stamped_ptr = identity; - return false; - } - return true; -} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 2ad4a47b833d1..b4d656cb1810b 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -30,7 +30,6 @@ ament_auto_add_executable(ndt_scan_matcher src/particle.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/map_module.cpp src/map_update_module.cpp ) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index d7a7b1c5c37f3..95a7cebdc01c8 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -18,7 +18,6 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | ----------------------------------- | ----------------------------------------------- | ------------------------------------- | | `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | initial pose | -| `pointcloud_map` | `sensor_msgs::msg::PointCloud2` | map pointcloud | | `points_raw` | `sensor_msgs::msg::PointCloud2` | sensor pointcloud | | `sensing/gnss/pose_with_covariance` | `sensor_msgs::msg::PoseWithCovarianceStamped` | base position for regularization term | @@ -193,12 +192,6 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Additional interfaces -#### Additional inputs - -| Name | Type | Description | -| ---------------- | ------------------------- | ----------------------------------------------------------- | -| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | - #### Additional outputs | Name | Type | Description | @@ -213,20 +206,15 @@ Using the feature, `ndt_scan_matcher` can theoretically handle any large size ma ### Parameters -| Name | Type | Description | -| ------------------------------------- | ------ | -------------------------------------------------------------------- | -| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | -| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | -| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | -| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | - -### Enabling the dynamic map loading feature +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------ | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | -To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. -Follow the next two instructions. +### Notes for dynamic map loading -1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) -2. split the PCD files into grids (recommended size: 20[m] x 20[m]) +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to split the PCD files into grids (recommended size: 20[m] x 20[m]) Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of @@ -235,14 +223,10 @@ Note that the dynamic map loading may FAIL if the map is split into two or more Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) -| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | -| :------------: | :-----------------------: | :------------------------: | :------------------: | -| single file | true | true | at once (standard) | -| single file | true | false | **does NOT work** | -| single file | false | true/false | at once (standard) | -| multiple files | true | true | dynamically | -| multiple files | true | false | **does NOT work** | -| multiple files | false | true/false | at once (standard) | +| PCD files | How NDT loads map(s) | +| :------------: | :------------------: | +| single file | at once (standard) | +| multiple files | dynamically | ## Scan matching score based on de-grounded LiDAR scan diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 1a6c26cd9c351..a5a4941bfec62 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,8 +1,5 @@ /**: ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true - # Vehicle reference frame base_frame: "base_link" @@ -57,6 +54,9 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100.0 + # Number of threads used for parallel computing num_threads: 4 @@ -101,6 +101,3 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 - - # The execution time which means probably NDT cannot matches scans properly. [ms] - critical_upper_bound_exe_time_ms: 100 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp deleted file mode 100644 index 79454e89b9ed0..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT_SCAN_MATCHER__MAP_MODULE_HPP_ -#define NDT_SCAN_MATCHER__MAP_MODULE_HPP_ - -#include - -#include - -#include -#include -#include - -#include - -class MapModule -{ - using PointSource = pcl::PointXYZ; - using PointTarget = pcl::PointXYZ; - using NormalDistributionsTransform = - pclomp::MultiGridNormalDistributionsTransform; - -public: - MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group); - -private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); - - rclcpp::Subscription::SharedPtr map_points_sub_; - std::shared_ptr ndt_ptr_; - std::mutex * ndt_ptr_mutex_; -}; - -#endif // NDT_SCAN_MATCHER__MAP_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index da0ae27a7808e..8b192b0186b42 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -15,7 +15,6 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "localization_util/tf2_listener_module.hpp" #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" @@ -48,14 +47,10 @@ class MapUpdateModule public: MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group); + std::shared_ptr ndt_ptr); private: friend class NDTScanMatcher; - void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); - void map_update_timer_callback(); void update_ndt( const std::vector & maps_to_add, @@ -68,21 +63,13 @@ class MapUpdateModule rclcpp::Client::SharedPtr pcd_loader_client_; - rclcpp::TimerBase::SharedPtr map_update_timer_; - - rclcpp::Subscription::SharedPtr ekf_odom_sub_; - - rclcpp::CallbackGroup::SharedPtr map_callback_group_; std::shared_ptr ndt_ptr_; std::mutex * ndt_ptr_mutex_; - std::string map_frame_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; - std::shared_ptr tf2_listener_module_; std::optional last_update_position_ = std::nullopt; - std::optional current_position_ = std::nullopt; const double dynamic_map_loading_update_distance_; const double dynamic_map_loading_map_radius_; const double lidar_radius_; 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 7f4a17a65a7e1..e2503c20aef6e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -18,8 +18,6 @@ #define FMT_HEADER_ONLY #include "localization_util/smart_pose_buffer.hpp" -#include "localization_util/tf2_listener_module.hpp" -#include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" #include @@ -41,6 +39,8 @@ #include #include #include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -88,6 +88,7 @@ class NDTScanMatcher : public rclcpp::Node const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); + void callback_timer(); void callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame); void callback_initial_pose( @@ -124,7 +125,10 @@ class NDTScanMatcher : public rclcpp::Node const double score, const double score_threshold, const std::string & score_name); bool validate_converged_param( 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); @@ -133,6 +137,7 @@ class NDTScanMatcher : public rclcpp::Node void publish_diagnostic(); + rclcpp::TimerBase::SharedPtr map_update_timer_; rclcpp::Subscription::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; rclcpp::Subscription::SharedPtr @@ -173,6 +178,10 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Service::SharedPtr service_trigger_node_; tf2_ros::TransformBroadcaster tf2_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; std::shared_ptr ndt_ptr_; std::shared_ptr> state_ptr_; @@ -191,8 +200,6 @@ class NDTScanMatcher : public rclcpp::Node double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; - float inversion_vector_threshold_; - float oscillation_threshold_; bool use_cov_estimation_; std::vector initial_pose_offset_model_; std::array output_pose_covariance_; @@ -200,13 +207,16 @@ class NDTScanMatcher : public rclcpp::Node std::mutex ndt_ptr_mtx_; std::unique_ptr initial_pose_buffer_; + // Keep latest position for dynamic map loading + // This variable is only used when use_dynamic_map_loading is true + std::mutex latest_ekf_position_mtx_; + std::optional latest_ekf_position_ = std::nullopt; + // variables for regularization const bool regularization_enabled_; // whether to use longitudinal regularization std::unique_ptr regularization_pose_buffer_; - bool is_activated_; - std::shared_ptr tf2_listener_module_; - std::unique_ptr map_module_; + std::atomic is_activated_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; @@ -214,10 +224,8 @@ class NDTScanMatcher : public rclcpp::Node bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; - bool use_dynamic_map_loading_; - // The execution time which means probably NDT cannot matches scans properly - int64_t critical_upper_bound_exe_time_ms_; + double critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ 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 b442ac1b3d843..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,32 +2,28 @@ - - - - + + + - - + - - + - + + - - + - 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/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp deleted file mode 100644 index d6088a1b14949..0000000000000 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt_scan_matcher/map_module.hpp" - -MapModule::MapModule( - rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex) -{ - auto map_sub_opt = rclcpp::SubscriptionOptions(); - map_sub_opt.callback_group = map_callback_group; - - map_points_sub_ = node->create_subscription( - "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MapModule::callback_map_points, this, std::placeholders::_1), map_sub_opt); -} - -void MapModule::callback_map_points( - sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr) -{ - NormalDistributionsTransform new_ndt; - new_ndt.setParams(ndt_ptr_->getParams()); - - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); - new_ndt.setInputTarget(map_points_ptr); - // create Thread - // detach - auto output_cloud = std::make_shared>(); - new_ndt.align(*output_cloud); - - // swap - ndt_ptr_mutex_->lock(); - *ndt_ptr_ = new_ndt; - ndt_ptr_mutex_->unlock(); -} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index f0a583417fb76..39b92fe248660 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -14,90 +14,37 @@ #include "ndt_scan_matcher/map_update_module.hpp" -template -double norm_xy(const T p1, const U p2) -{ - double dx = p1.x - p2.x; - double dy = p1.y - p2.y; - return std::sqrt(dx * dx + dy * dy); -} - MapUpdateModule::MapUpdateModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, - std::shared_ptr ndt_ptr, - std::shared_ptr tf2_listener_module, std::string map_frame, - rclcpp::CallbackGroup::SharedPtr main_callback_group) + std::shared_ptr ndt_ptr) : ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), - map_frame_(std::move(map_frame)), logger_(node->get_logger()), clock_(node->get_clock()), - tf2_listener_module_(std::move(tf2_listener_module)), dynamic_map_loading_update_distance_( node->declare_parameter("dynamic_map_loading_update_distance")), dynamic_map_loading_map_radius_( node->declare_parameter("dynamic_map_loading_map_radius")), lidar_radius_(node->declare_parameter("lidar_radius")) { - auto main_sub_opt = rclcpp::SubscriptionOptions(); - main_sub_opt.callback_group = main_callback_group; - - map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - ekf_odom_sub_ = node->create_subscription( - "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), - main_sub_opt); - loaded_pcd_pub_ = node->create_publisher( "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); pcd_loader_client_ = node->create_client("pcd_loader_service"); - - double map_update_dt = 1.0; - auto period_ns = std::chrono::duration_cast( - std::chrono::duration(map_update_dt)); - map_update_timer_ = rclcpp::create_timer( - node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), - map_callback_group_); } -void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const { - current_position_ = odom_ptr->pose.pose.position; - if (last_update_position_ == std::nullopt) { - return; + return false; } - double distance = norm_xy(current_position_.value(), last_update_position_.value()); + const double dx = position.x - last_update_position_.value().x; + const double dy = position.y - last_update_position_.value().y; + const double distance = std::hypot(dx, dy); if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { - RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); - } -} - -void MapUpdateModule::map_update_timer_callback() -{ - if (current_position_ == std::nullopt) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 1, - "Cannot find the reference position for map update. Please check if the EKF odometry is " - "provided to NDT."); - return; + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, "Dynamic map loading is not keeping up."); } - if (last_update_position_ == std::nullopt) return; - - // continue only if we should update the map - if (should_update_map(current_position_.value())) { - RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); - update_map(current_position_.value()); - last_update_position_ = current_position_; - } -} - -bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const -{ - if (last_update_position_ == std::nullopt) return false; - double distance = norm_xy(position, last_update_position_.value()); return distance > dynamic_map_loading_update_distance_; } @@ -110,10 +57,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->cached_ids = ndt_ptr_->getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); + RCLCPP_INFO(logger_, "Waiting for pcd loader service. Check the pointcloud_map_loader."); } // send a request to map_loader @@ -145,21 +89,30 @@ void MapUpdateModule::update_ndt( } const auto exe_start_time = std::chrono::system_clock::now(); - NormalDistributionsTransform backup_ndt = *ndt_ptr_; + const size_t add_size = maps_to_add.size(); + + // Perform heavy processing outside of the lock scope + std::vector>> points_pcl(add_size); + for (size_t i = 0; i < add_size; i++) { + points_pcl[i] = pcl::make_shared>(); + pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); + } + + (*ndt_ptr_mutex_).lock(); // Add pcd - for (const auto & map_to_add : maps_to_add) { - pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); - pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); - backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + for (size_t i = 0; i < add_size; i++) { + ndt_ptr_->addTarget(points_pcl[i], maps_to_add[i].cell_id); } // Remove pcd for (const std::string & map_id_to_remove : map_ids_to_remove) { - backup_ndt.removeTarget(map_id_to_remove); + ndt_ptr_->removeTarget(map_id_to_remove); } - backup_ndt.createVoxelKdtree(); + ndt_ptr_->createVoxelKdtree(); + + (*ndt_ptr_mutex_).unlock(); const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = @@ -167,14 +120,6 @@ void MapUpdateModule::update_ndt( const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); - // swap - (*ndt_ptr_mutex_).lock(); - // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should - // ideally be avoided. But I will leave this for now since I cannot come up with a solution other - // than using pointer of pointer. - *ndt_ptr_ = backup_ndt; - (*ndt_ptr_mutex_).unlock(); - publish_partial_pcd_map(); } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 783df34e6c0f8..0382d805b7276 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -64,47 +64,19 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -bool validate_local_optimal_solution_oscillation( - const std::vector & result_pose_msg_array, - const float oscillation_threshold, const float inversion_vector_threshold) -{ - bool prev_oscillation = false; - int oscillation_cnt = 0; - - for (size_t i = 2; i < result_pose_msg_array.size(); ++i) { - const Eigen::Vector3d current_pose = point_to_vector3d(result_pose_msg_array.at(i).position); - const Eigen::Vector3d prev_pose = point_to_vector3d(result_pose_msg_array.at(i - 1).position); - const Eigen::Vector3d prev_prev_pose = - point_to_vector3d(result_pose_msg_array.at(i - 2).position); - const auto current_vec = current_pose - prev_pose; - const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); - const bool oscillation = prev_vec.dot(current_vec) < inversion_vector_threshold; - if (prev_oscillation && oscillation) { - if (static_cast(oscillation_cnt) > oscillation_threshold) { - return true; - } - ++oscillation_cnt; - } else { - oscillation_cnt = 0; - } - prev_oscillation = oscillation; - } - return false; -} - // cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), + tf2_buffer_(this->get_clock()), + tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - inversion_vector_threshold_(-0.9), // Not necessary to extract to ndt_scan_matcher.param.yaml - oscillation_threshold_(10), // Not necessary to extract to ndt_scan_matcher.param.yaml output_pose_covariance_({}), - regularization_enabled_(declare_parameter("regularization_enabled")) + regularization_enabled_(declare_parameter("regularization_enabled")), + is_activated_(false) { (*state_ptr_)["state"] = "Initializing"; - is_activated_ = false; int64_t points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); points_queue_size = std::max(points_queue_size, (int64_t)0); @@ -146,7 +118,7 @@ NDTScanMatcher::NDTScanMatcher() lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms"); + this->declare_parameter("critical_upper_bound_exe_time_ms"); initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); @@ -193,31 +165,36 @@ NDTScanMatcher::NDTScanMatcher() z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); - rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group; - initial_pose_callback_group = + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroup::SharedPtr sensor_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - rclcpp::CallbackGroup::SharedPtr main_callback_group; - main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); auto initial_pose_sub_opt = rclcpp::SubscriptionOptions(); initial_pose_sub_opt.callback_group = initial_pose_callback_group; - - auto main_sub_opt = rclcpp::SubscriptionOptions(); - main_sub_opt.callback_group = main_callback_group; - + auto sensor_sub_opt = rclcpp::SubscriptionOptions(); + sensor_sub_opt.callback_group = sensor_callback_group; + + constexpr double map_update_dt = 1.0; + constexpr auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + this, this->get_clock(), period_ns, std::bind(&NDTScanMatcher::callback_timer, this), + timer_callback_group_); initial_pose_sub_ = this->create_subscription( "ekf_pose_with_covariance", 100, std::bind(&NDTScanMatcher::callback_initial_pose, this, std::placeholders::_1), initial_pose_sub_opt); sensor_points_sub_ = this->create_subscription( "points_raw", rclcpp::SensorDataQoS().keep_last(points_queue_size), - std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), main_sub_opt); + std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), + sensor_sub_opt); // Only if regularization is enabled, subscribe to the regularization base pose if (regularization_enabled_) { // NOTE: The reason that the regularization subscriber does not belong to the - // main_callback_group is to ensure that the regularization callback is called even if + // sensor_callback_group is to ensure that the regularization callback is called even if // sensor_callback takes long time to process. // Both callback_initial_pose and callback_regularization_pose must not miss receiving data for // proper interpolation. @@ -280,22 +257,14 @@ NDTScanMatcher::NDTScanMatcher() "ndt_align_srv", std::bind( &NDTScanMatcher::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group); + rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); service_trigger_node_ = this->create_service( "trigger_node_srv", std::bind( &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), - rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group); + rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); - tf2_listener_module_ = std::make_shared(this); - - use_dynamic_map_loading_ = this->declare_parameter("use_dynamic_map_loading"); - if (use_dynamic_map_loading_) { - map_update_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group); - } else { - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - } + map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_); logger_configure_ = std::make_unique(this); } @@ -347,8 +316,7 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= - static_cast(critical_upper_bound_exe_time_ms_)) { + std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; @@ -368,6 +336,26 @@ void NDTScanMatcher::publish_diagnostic() diagnostics_pub_->publish(diag_msg); } +void NDTScanMatcher::callback_timer() +{ + if (!is_activated_) { + return; + } + std::lock_guard lock(latest_ekf_position_mtx_); + if (latest_ekf_position_ == std::nullopt) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Cannot find the reference position for map update. Please check if the EKF odometry is " + "provided to NDT."); + return; + } + // continue only if we should update the map + if (map_update_module_->should_update_map(latest_ekf_position_.value())) { + RCLCPP_INFO(this->get_logger(), "Start updating NDT map (timer_callback)"); + map_update_module_->update_map(latest_ekf_position_.value()); + } +} + void NDTScanMatcher::callback_initial_pose( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { @@ -382,6 +370,12 @@ void NDTScanMatcher::callback_initial_pose( << initial_pose_msg_ptr->header.frame_id << ", but expected " << map_frame_ << ". Please check the frame_id in the input topic and ensure it is correct."); } + + { + // latest_ekf_position_ is also used by callback_timer, so it is necessary to acquire the lock + std::lock_guard lock(latest_ekf_position_mtx_); + latest_ekf_position_ = initial_pose_msg_ptr->pose.pose.position; + } } void NDTScanMatcher::callback_regularization_pose( @@ -473,10 +467,11 @@ void NDTScanMatcher::callback_sensor_points( // perform several validations bool is_ok_iteration_num = validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations()); + const int oscillation_num = count_oscillation(transformation_msg_array); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { - is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( - transformation_msg_array, oscillation_threshold_, inversion_vector_threshold_); + constexpr int oscillation_threshold = 10; + is_local_optimal_solution_oscillation = (oscillation_num > oscillation_threshold); } bool is_ok_converged_param = validate_converged_param( ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood); @@ -490,7 +485,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); @@ -558,11 +561,9 @@ void NDTScanMatcher::callback_sensor_points( std::to_string(ndt_result.nearest_voxel_transformation_likelihood); (*state_ptr_)["iteration_num"] = std::to_string(ndt_result.iteration_num); (*state_ptr_)["skipping_publish_num"] = std::to_string(skipping_publish_num); - if (is_local_optimal_solution_oscillation) { - (*state_ptr_)["is_local_optimal_solution_oscillation"] = "1"; - } else { - (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; - } + (*state_ptr_)["oscillation_count"] = std::to_string(oscillation_num); + (*state_ptr_)["is_local_optimal_solution_oscillation"] = + std::to_string(is_local_optimal_solution_oscillation); (*state_ptr_)["execution_time"] = std::to_string(exe_time); publish_diagnostic(); @@ -573,11 +574,25 @@ void NDTScanMatcher::transform_sensor_measurement( const pcl::shared_ptr> & sensor_points_input_ptr, pcl::shared_ptr> & sensor_points_output_ptr) { - auto tf_target_to_source_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - this->now(), target_frame, source_frame, tf_target_to_source_ptr); + if (source_frame == target_frame) { + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + // Since there is no clear error handling policy, temporarily return as is. + sensor_points_output_ptr = sensor_points_input_ptr; + return; + } + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = - tier4_autoware_utils::transform2pose(*tf_target_to_source_ptr); + tier4_autoware_utils::transform2pose(transform); const Eigen::Matrix4f base_to_sensor_matrix = pose_to_matrix4f(target_to_source_pose_stamped.pose); tier4_autoware_utils::transformPointCloud( @@ -731,6 +746,55 @@ bool NDTScanMatcher::validate_converged_param( return is_ok_converged_param; } +int NDTScanMatcher::count_oscillation( + const std::vector & result_pose_msg_array) +{ + constexpr double inversion_vector_threshold = -0.9; + + int oscillation_cnt = 0; + int max_oscillation_cnt = 0; + + for (size_t i = 2; i < result_pose_msg_array.size(); ++i) { + const Eigen::Vector3d current_pose = point_to_vector3d(result_pose_msg_array.at(i).position); + const Eigen::Vector3d prev_pose = point_to_vector3d(result_pose_msg_array.at(i - 1).position); + const Eigen::Vector3d prev_prev_pose = + point_to_vector3d(result_pose_msg_array.at(i - 2).position); + const auto current_vec = (current_pose - prev_pose).normalized(); + const auto prev_vec = (prev_pose - prev_prev_pose).normalized(); + const double cosine_value = current_vec.dot(prev_vec); + const bool oscillation = cosine_value < inversion_vector_threshold; + if (oscillation) { + oscillation_cnt++; // count consecutive oscillation + } else { + oscillation_cnt = 0; // reset + } + max_oscillation_cnt = std::max(max_oscillation_cnt, oscillation_cnt); + } + 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) @@ -835,17 +899,29 @@ void NDTScanMatcher::service_ndt_align( tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) { // get TF from pose_frame to map_frame - auto tf_pose_to_map_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - get_clock()->now(), map_frame_, req->pose_with_covariance.header.frame_id, tf_pose_to_map_ptr); + const std::string & target_frame = map_frame_; + const std::string & source_frame = req->pose_with_covariance.header.frame_id; - // transform pose_frame to map_frame - const auto initial_pose_msg_in_map_frame = - transform(req->pose_with_covariance, *tf_pose_to_map_ptr); - if (use_dynamic_map_loading_) { - map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + geometry_msgs::msg::TransformStamped transform_s2t; + try { + transform_s2t = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + // Note: Up to AWSIMv1.1.0, there is a known bug where the GNSS frame_id is incorrectly set to + // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue + // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + transform_s2t.header.stamp = get_clock()->now(); + transform_s2t.header.frame_id = target_frame; + transform_s2t.child_frame_id = source_frame; + transform_s2t.transform = tf2::toMsg(tf2::Transform::getIdentity()); } + // transform pose_frame to map_frame + const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); + map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); + // mutex Map std::lock_guard lock(ndt_ptr_mtx_); 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 f50eba9218d67..85258a8e54b22 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -7,6 +7,11 @@ Takagi, Isamu 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_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 1eb1edd04a68a..a05c63d44ebce 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -101,11 +101,11 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) req->area.center_y = point.y; req->area.radius = 50; - RCLCPP_INFO(logger, "Send request to map_loader"); + RCLCPP_DEBUG(logger, "Send request to map_loader"); auto future = cli_map_->async_send_request(req); auto status = future.wait_for(std::chrono::seconds(1)); while (status != std::future_status::ready) { - RCLCPP_INFO(logger, "waiting response"); + RCLCPP_DEBUG(logger, "waiting response"); if (!rclcpp::ok()) { return false; } @@ -113,7 +113,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } const auto res = future.get(); - RCLCPP_INFO( + RCLCPP_DEBUG( logger, "Loaded partial pcd map from map_loader (grid size: %lu)", res->new_pointcloud_with_ids.size()); @@ -168,7 +168,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st const auto logger = node_->get_logger(); tf2::Vector3 point(position.x, position.y, position.z); - RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); if (cli_map_) { if (!get_partial_point_cloud_map(position)) { @@ -193,7 +193,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st return std::nullopt; } - RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); Point result; result.x = point.getX(); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 65d9594cb7415..fbe019096a3e7 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,7 +20,7 @@ NOTE: **We strongly recommend to use divided maps when using large pointcloud ma #### Prerequisites on pointcloud map file(s) -You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules: +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: 1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). 2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. @@ -29,8 +29,6 @@ You may provide either a single .pcd file or multiple .pcd files. If you are usi 5. **All the split maps should not overlap with each other.** 6. **Metadata file should also be provided.** The metadata structure description is provided below. -Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023). - #### Metadata structure The metadata should look like this: @@ -118,7 +116,6 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | false | | enable_partial_load | bool | A flag to enable partial pointcloud map server | false | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | false | | enable_selected_load | bool | A flag to enable selected pointcloud map server | false | | leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | | pcd_paths_or_directory | std::string | Path(s) to pointcloud map file or directory | | diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index ba4c032d3e514..b4efbec9706b4 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,7 +3,6 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: true - enable_differential_load: true enable_selected_load: false # only used when downsample_whole_load enabled diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 5230d4bd03214..0103c7d2b74a7 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -8,6 +8,10 @@ Ryu Yamamoto 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_local_projector.hpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp new file mode 100644 index 0000000000000..225445d17bfa1 --- /dev/null +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp @@ -0,0 +1,41 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ +#define LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ + +#include + +namespace lanelet::projection +{ + +class LocalProjector : public Projector +{ +public: + LocalProjector() : Projector(Origin(GPSPoint{})) {} + + BasicPoint3d forward(const GPSPoint & gps) const override // NOLINT + { + return BasicPoint3d{0.0, 0.0, gps.ele}; + } + + GPSPoint reverse(const BasicPoint3d & point) const override + { + return GPSPoint{0.0, 0.0, point.z()}; + } +}; + +} // namespace lanelet::projection + +#endif // LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 259c168edcc5c..5db00fe0ad2f5 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -33,6 +33,8 @@ #include "map_loader/lanelet2_map_loader_node.hpp" +#include "lanelet2_local_projector.hpp" + #include #include #include @@ -100,8 +102,7 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return map; } } else { - // Use MGRSProjector as parser - lanelet::projection::MGRSProjector projector{}; + const lanelet::projection::LocalProjector projector; const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); // overwrite local_x, local_y 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 082dc95d6500a..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)); @@ -224,6 +230,9 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index a2d9307084545..5f4b3e311e6e9 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -53,7 +53,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_differential_load = declare_parameter("enable_differential_load"); bool enable_selected_load = declare_parameter("enable_selected_load"); if (enable_whole_load) { @@ -68,26 +67,21 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - if (enable_partial_load || enable_differential_load || enable_selected_load) { - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } + std::map pcd_metadata_dict; + try { + pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); + } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict); - } + differential_map_loader_ = std::make_unique(this, pcd_metadata_dict); - if (enable_selected_load) { - selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } + if (enable_selected_load) { + selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); } } diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 94b142da3dcf9..1887a1cd8934f 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -28,6 +28,15 @@ sample-map-rosbag projector_type: local ``` +#### Limitation + +The functionality that requires latitude and longitude will become unavailable. + +The currently identified unavailable functionalities are: + +- GNSS localization +- Sending the self-position in latitude and longitude using ADAPI + ### Using MGRS If you want to use MGRS, please specify the MGRS grid as well. @@ -39,6 +48,10 @@ vertical_datum: WGS84 mgrs_grid: 54SUE ``` +#### Limitation + +It cannot be used with maps that span across two or more MGRS grids. Please use it only when it falls within the scope of a single MGRS grid. + ### Using LocalCartesianUTM If you want to use local cartesian UTM, please specify the map origin as well. diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 16442eb43c740..54e794e2742bf 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -23,6 +23,8 @@ #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename); class MapProjectionLoader : public rclcpp::Node { diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index b128c2cf15e15..475881577bd58 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/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 ament_cmake_auto diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 9fdba288601cc..5966baaed8383 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -20,6 +20,7 @@ #include +#include #include tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) @@ -55,28 +56,40 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename) { - std::string yaml_filename = this->declare_parameter("map_projector_info_path"); - std::string lanelet2_map_filename = this->declare_parameter("lanelet2_map_path"); - std::ifstream file(yaml_filename); - tier4_map_msgs::msg::MapProjectorInfo msg; - bool use_yaml_file = file.is_open(); - if (use_yaml_file) { - RCLCPP_INFO(this->get_logger(), "Load %s", yaml_filename.c_str()); + if (std::filesystem::exists(yaml_filename)) { + std::cout << "Load " << yaml_filename << std::endl; msg = load_info_from_yaml(yaml_filename); - } else { - RCLCPP_INFO(this->get_logger(), "Load %s", lanelet2_map_filename.c_str()); - RCLCPP_WARN( - this->get_logger(), - "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " - "Please use map_projector_info.yaml instead. For more info, visit " - "https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_projection_loader/" - "README.md"); + } else if (std::filesystem::exists(lanelet2_map_filename)) { + std::cout << "Load " << lanelet2_map_filename << std::endl; + std::cout + << "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader/" + "README.md" + << std::endl; msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } else { + throw std::runtime_error( + "No map projector info files found. Please provide either " + "map_projector_info.yaml or lanelet2_map.osm"); } + return msg; +} + +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +{ + const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + const std::string lanelet2_map_filename = + this->declare_parameter("lanelet2_map_path"); + + const tier4_map_msgs::msg::MapProjectorInfo msg = + load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message const auto adaptor = component_interface_utils::NodeAdaptor(this); diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index 6dc68911699aa..6f08da169309e 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -4,7 +4,13 @@ map_tf_generator 0.1.0 map_tf_generator package as a ROS 2 node - azumi-suzuki + 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/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index 4655cf4c2f73a..d56cb3fb31584 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -89,6 +89,8 @@ install( RUNTIME DESTINATION bin ) -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch + config ) diff --git a/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml new file mode 100644 index 0000000000000..3d5270d17479e --- /dev/null +++ b/perception/compare_map_segmentation/config/compare_elevation_map_filter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + map_layer_name: "elevation" + height_diff_thresh: 0.15 + map_frame: "map" diff --git a/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..f95c0b0c94b92 --- /dev/null +++ b/perception/compare_map_segmentation/config/distance_based_compare_map_filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml new file mode 100644 index 0000000000000..d54f7a96ecda0 --- /dev/null +++ b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + distance_threshold: 0.5 + use_dynamic_map_loading: true + downsize_ratio_z_axis: 0.5 + timer_interval_ms: 100 + map_update_distance_threshold: 10.0 + map_loader_radius: 150.0 + publish_debug_pcd: False diff --git a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml index 42478c514b788..9cb41c8780f13 100644 --- a/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/compare_elevation_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml index 4a272646503f1..cdf17ebfeda9f 100644 --- a/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/distance_based_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml index 2942781ce8123..e3f4594729c2b 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_approximate_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml index 2fc2884fd5df9..01ad5cd9b1792 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml @@ -1,25 +1,16 @@ + + - - - - - - - - - - - - + diff --git a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml index 09ea9022b97e5..13023a8c1c2c9 100644 --- a/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_distance_based_compare_map_filter.launch.xml @@ -1,11 +1,13 @@ + + - + diff --git a/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json new file mode 100644 index 0000000000000..24ac2481a4e4a --- /dev/null +++ b/perception/compare_map_segmentation/schema/compare_elevation_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Compare Elevation Map Filter", + "type": "object", + "definitions": { + "compare_elevation_map_filter": { + "type": "object", + "properties": { + "map_layer_name": { + "type": "string", + "default": "elevation", + "description": "elevation map layer name" + }, + "height_diff_thresh": { + "type": "number", + "default": "0.15", + "description": "Remove points whose height difference is below this value [m]" + }, + "map_frame": { + "type": "string", + "default": "map", + "description": "frame_id of the map that is temporarily used before elevation_map is subscribed" + } + }, + "required": ["map_layer_name", "height_diff_thresh", "map_frame"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/compare_elevation_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..10454b48d3d80 --- /dev/null +++ b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Distance Based Compare Map Filter", + "type": "object", + "definitions": { + "distance_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/distance_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json new file mode 100644 index 0000000000000..3c301d5ad9fbb --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Based Approximate Compare Map Filter", + "type": "object", + "definitions": { + "voxel_based_approximate_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_based_approximate_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..2e541662a1743 --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Based Compare Map Filter", + "type": "object", + "definitions": { + "voxel_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + }, + "map_update_distance_threshold": { + "type": "number", + "default": "10.0", + "description": "Threshold distance to update map points with input points [m]" + }, + "map_loader_radius": { + "type": "number", + "default": "150.0", + "description": "Radius to load map points [m]" + }, + "timer_interval_ms": { + "type": "number", + "default": "100", + "description": "Timer interval to load map points [ms]" + } + }, + "required": [ + "distance_threshold", + "use_dynamic_map_loading", + "downsize_ratio_z_axis", + "map_update_distance_threshold", + "map_loader_radius", + "timer_interval_ms" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json new file mode 100644 index 0000000000000..4663dbe806223 --- /dev/null +++ b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json @@ -0,0 +1,43 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Distance Based Compare Map Filter", + "type": "object", + "definitions": { + "voxel_distance_based_compare_map_filter": { + "type": "object", + "properties": { + "distance_threshold": { + "type": "number", + "default": "0.5", + "description": "Threshold distance to compare input points with map points [m]" + }, + "use_dynamic_map_loading": { + "type": "boolean", + "default": "true", + "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "downsize_ratio_z_axis": { + "type": "number", + "default": "0.5", + "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + } + }, + "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_distance_based_compare_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp index 7c8f2e8d4bd76..1eadeeb3bec05 100644 --- a/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp +++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter_node.cpp @@ -40,9 +40,9 @@ CompareElevationMapFilterComponent::CompareElevationMapFilterComponent( : Filter("CompareElevationMapFilter", options) { unsubscribe(); - layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); - height_diff_thresh_ = this->declare_parameter("height_diff_thresh", 0.15); - map_frame_ = this->declare_parameter("map_frame", "map"); + layer_name_ = declare_parameter("map_layer_name"); + height_diff_thresh_ = declare_parameter("height_diff_thresh"); + map_frame_ = declare_parameter("map_frame"); rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml index 33168c87de7eb..2be9074f8d71e 100644 --- a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml +++ b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml @@ -16,3 +16,4 @@ ros__parameters: use_last_detect_color: true last_detect_color_hold_time: 2.0 + last_colors_hold_time: 1.0 diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 848293f6334bb..0578d671d27b9 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -34,11 +34,11 @@ #include #include +#include #include #include #include #include - namespace traffic_light { @@ -53,6 +53,8 @@ using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement using TrafficSignalAndTime = std::pair; using TrafficLightIdMap = std::unordered_map; +using TrafficLightIdArray = std::unordered_map>; + class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node { public: @@ -76,8 +78,12 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg); void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals); + void updateLastDetectedSignals(const TrafficLightIdMap & traffic_signals); + void updateFlashingState(const TrafficSignal & signal); + uint8_t updateAndGetColorState(const TrafficSignal & signal); void setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const; + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output); lanelet::ConstLanelets getNonRedLanelets( const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const; @@ -97,9 +103,15 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; + double last_colors_hold_time_; // Signal history TrafficLightIdMap last_detect_color_; + TrafficLightIdArray last_colors_; + + // State + std::map is_flashing_; + std::map current_color_state_; // Stop watch StopWatch stop_watch_; diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 6527f0662bcbf..904a365786755 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -73,7 +73,6 @@ bool hasMergeLane( return false; } - } // namespace CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( @@ -84,6 +83,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( use_last_detect_color_ = declare_parameter("use_last_detect_color"); last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); + last_colors_hold_time_ = declare_parameter("last_colors_hold_time"); sub_map_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), @@ -164,19 +164,19 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( traffic_light_id_map[traffic_signal.traffic_signal_id] = std::pair(traffic_signal, get_clock()->now()); } - for (const auto & crosswalk : conflicting_crosswalks_) { constexpr int VEHICLE_GRAPH_ID = 0; const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID); const auto non_red_lanelets = getNonRedLanelets(conflict_lls, traffic_light_id_map); const auto crosswalk_tl_color = estimateCrosswalkTrafficSignal(crosswalk, non_red_lanelets); - setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, output); + setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, *msg, output); } removeDuplicateIds(output); updateLastDetectedSignal(traffic_light_id_map); + updateLastDetectedSignals(traffic_light_id_map); pub_traffic_light_array_->publish(output); pub_processing_time_->publish("processing_time_ms", stop_watch.toc("Total")); @@ -221,24 +221,160 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( } } } - for (const auto id : erase_id_list) last_detect_color_.erase(id); + for (const auto id : erase_id_list) { + last_detect_color_.erase(id); + is_flashing_.erase(id); + current_color_state_.erase(id); + } +} + +void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignals( + const TrafficLightIdMap & traffic_light_id_map) +{ + for (const auto & input_traffic_signal : traffic_light_id_map) { + const auto & elements = input_traffic_signal.second.first.elements; + + if (elements.empty()) { + continue; + } + + if ( + elements.front().color == TrafficSignalElement::UNKNOWN && elements.front().confidence == 1) { + continue; + } + + const auto & id = input_traffic_signal.second.first.traffic_signal_id; + + if (last_colors_.count(id) == 0) { + std::vector signal{input_traffic_signal.second}; + last_colors_.insert(std::make_pair(id, signal)); + continue; + } + + last_colors_.at(id).push_back(input_traffic_signal.second); + } + + std::vector erase_id_list; + for (auto & last_traffic_signal : last_colors_) { + const auto & id = last_traffic_signal.first; + for (auto it = last_traffic_signal.second.begin(); it != last_traffic_signal.second.end();) { + auto sig = (*it).first; + rclcpp::Time t = (*it).second; + + // hold signal recognition results for [last_colors_hold_time_] seconds. + const auto time_from_last_detected = (get_clock()->now() - t).seconds(); + if (time_from_last_detected > last_colors_hold_time_) { + it = last_traffic_signal.second.erase(it); + } else { + ++it; + } + } + if (last_traffic_signal.second.empty()) { + erase_id_list.emplace_back(id); + } + } + for (const auto id : erase_id_list) last_colors_.erase(id); } void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output) { const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); + std::unordered_map valid_id2idx_map; // detected traffic light + + for (size_t i = 0; i < msg.signals.size(); ++i) { + auto signal = msg.signals[i]; + valid_id2idx_map[signal.traffic_signal_id] = i; + } + for (const auto & tl_reg_elem : tl_reg_elems) { - TrafficSignal output_traffic_signal; - TrafficSignalElement output_traffic_signal_element; - output_traffic_signal_element.color = color; - output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; - output_traffic_signal_element.confidence = 1.0; - output_traffic_signal.elements.push_back(output_traffic_signal_element); - output_traffic_signal.traffic_signal_id = tl_reg_elem->id(); - msg.signals.push_back(output_traffic_signal); + auto id = tl_reg_elem->id(); + // if valid prediction exists, overwrite the estimation; else, use the estimation + if (valid_id2idx_map.count(id)) { + size_t idx = valid_id2idx_map[id]; + auto signal = msg.signals[idx]; + updateFlashingState(signal); // check if it is flashing + // update output msg according to flashing and current state + output.signals[idx].elements[0].color = updateAndGetColorState(signal); + } else { + TrafficSignal output_traffic_signal; + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; + output_traffic_signal_element.confidence = 1.0; + output_traffic_signal.elements.push_back(output_traffic_signal_element); + output_traffic_signal.traffic_signal_id = id; + output.signals.push_back(output_traffic_signal); + } + } +} + +void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_signal_id; + + // no record of detected color in last_detect_color_hold_time_ + if (is_flashing_.count(id) == 0) { + is_flashing_.insert(std::make_pair(id, false)); + return; + } + + // flashing green + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence != 0 && // not due to occlusion + current_color_state_.at(id) != TrafficSignalElement::UNKNOWN) { + is_flashing_.at(id) = true; + return; + } + + // history exists + if (last_colors_.count(id) > 0) { + std::vector history = last_colors_.at(id); + for (const auto & h : history) { + if (h.first.elements.front().color != signal.elements.front().color) { + // keep the current value if not same with input signal + return; + } + } + // all history is same with input signal + is_flashing_.at(id) = false; + } + + // no record of detected color in last_color_hold_time_ + // keep the current value + return; +} + +uint8_t CrosswalkTrafficLightEstimatorNode::updateAndGetColorState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_signal_id; + const auto color = signal.elements[0].color; + + if (current_color_state_.count(id) == 0) { + current_color_state_.insert(std::make_pair(id, color)); + } else if (is_flashing_.at(id) == false) { + current_color_state_.at(id) = color; + } else if (is_flashing_.at(id) == true) { + if ( + current_color_state_.at(id) == TrafficSignalElement::GREEN && + color == TrafficSignalElement::RED) { + current_color_state_.at(id) = TrafficSignalElement::RED; + } else if ( + current_color_state_.at(id) == TrafficSignalElement::RED && + color == TrafficSignalElement::GREEN) { + current_color_state_.at(id) = TrafficSignalElement::GREEN; + } else if (current_color_state_.at(id) == TrafficSignalElement::UNKNOWN) { + if (color == TrafficSignalElement::GREEN || color == TrafficSignalElement::UNKNOWN) + current_color_state_.at(id) = TrafficSignalElement::GREEN; + if (color == TrafficSignalElement::RED) + current_color_state_.at(id) = TrafficSignalElement::RED; + } } + + return current_color_state_.at(id); } lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp similarity index 88% rename from perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp rename to perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index ae6162542a1c3..4331b5c19d3f1 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#define DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include #include @@ -69,4 +69,4 @@ class ObjectLaneletFilterNode : public rclcpp::Node } // namespace object_lanelet_filter -#endif // DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp similarity index 84% rename from perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp rename to perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index b9384e0f70379..be25eb6a353e6 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#define DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#define DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ -#include "utils/utils.hpp" +#include "detected_object_validation/utils/utils.hpp" #include #include @@ -55,4 +55,4 @@ class ObjectPositionFilterNode : public rclcpp::Node } // namespace object_position_filter -#endif // DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp similarity index 93% rename from perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp rename to perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp index 6597475ae297e..8f5fa054090f5 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ -#define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ #include @@ -113,4 +113,4 @@ class Debugger }; } // namespace obstacle_pointcloud_based_validator -#endif // OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp similarity index 90% rename from perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp rename to perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index e7df2c409b18f..2d70247445043 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ -#define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#ifndef DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#define DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ -#include "obstacle_pointcloud_based_validator/debugger.hpp" +#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp" #include @@ -152,4 +154,5 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node }; } // namespace obstacle_pointcloud_based_validator -#endif // OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ +// NOLINTNEXTLINE(whitespace/line_length) +#endif // DETECTED_OBJECT_VALIDATION__OBSTACLE_POINTCLOUD_BASED_VALIDATOR__OBSTACLE_POINTCLOUD_BASED_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp similarity index 89% rename from perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp rename to perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index f62b0d866dc3a..5fd866d09e725 100644 --- a/perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ -#define OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#define DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ #include #include @@ -68,4 +68,4 @@ class OccupancyGridBasedValidator : public rclcpp::Node }; } // namespace occupancy_grid_based_validator -#endif // OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__OCCUPANCY_GRID_BASED_VALIDATOR__OCCUPANCY_GRID_BASED_VALIDATOR_HPP_ diff --git a/perception/detected_object_validation/include/utils/utils.hpp b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp similarity index 84% rename from perception/detected_object_validation/include/utils/utils.hpp rename to perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp index 21df77825176b..2c46e1b9dd110 100644 --- a/perception/detected_object_validation/include/utils/utils.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ +#define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ #include @@ -33,4 +33,4 @@ struct FilterTargetLabel }; // struct FilterTargetLabel } // namespace utils -#endif // UTILS__UTILS_HPP_ +#endif // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_ diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index fe8de37c778ac..cd16d414425cb 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_filter/object_lanelet_filter.hpp" +#include "detected_object_validation/detected_object_filter/object_lanelet_filter.hpp" #include #include diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 0f59e60d57d55..a509fc7571dd5 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "detected_object_filter/object_position_filter.hpp" +#include "detected_object_validation/detected_object_filter/object_position_filter.hpp" namespace object_position_filter { diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 3249581513dd9..d903a9ca3bb41 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" +#include "detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" #include #include diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 948f040d7ebde..80e4dc66d35c2 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" +#include "detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp" #include #include diff --git a/perception/detected_object_validation/src/utils.cpp b/perception/detected_object_validation/src/utils.cpp index d8f438e265c3a..53b21ee7ff5b6 100644 --- a/perception/detected_object_validation/src/utils.cpp +++ b/perception/detected_object_validation/src/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 "detected_object_validation/utils/utils.hpp" #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 10affd0b94ffd..771747c80bb95 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -16,6 +16,7 @@ #define DETECTION_BY_TRACKER__DETECTION_BY_TRACKER_CORE_HPP_ #include "detection_by_tracker/debugger.hpp" +#include "detection_by_tracker/utils.hpp" #include #include @@ -39,8 +40,6 @@ #include #endif -#include "utils/utils.hpp" - #include #include @@ -82,7 +81,7 @@ class DetectionByTracker : public rclcpp::Node std::map max_search_distance_for_merger_; std::map max_search_distance_for_divider_; - utils::TrackerIgnoreLabel tracker_ignore_; + detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; void setMaxSearchRange(); diff --git a/perception/detection_by_tracker/include/utils/utils.hpp b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp similarity index 81% rename from perception/detection_by_tracker/include/utils/utils.hpp rename to perception/detection_by_tracker/include/detection_by_tracker/utils.hpp index 3f39125b95e03..ed783b0343a14 100644 --- a/perception/detection_by_tracker/include/utils/utils.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/utils.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILS__UTILS_HPP_ -#define UTILS__UTILS_HPP_ +#ifndef DETECTION_BY_TRACKER__UTILS_HPP_ +#define DETECTION_BY_TRACKER__UTILS_HPP_ #include +namespace detection_by_tracker +{ namespace utils { struct TrackerIgnoreLabel @@ -32,5 +34,6 @@ struct TrackerIgnoreLabel bool isIgnore(const uint8_t label) const; }; // struct TrackerIgnoreLabel } // namespace utils +} // namespace detection_by_tracker -#endif // UTILS__UTILS_HPP_ +#endif // DETECTION_BY_TRACKER__UTILS_HPP_ diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp index 29a500a24cf32..b2655d99abacc 100644 --- a/perception/detection_by_tracker/src/utils.cpp +++ b/perception/detection_by_tracker/src/utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utils/utils.hpp" +#include "detection_by_tracker/utils.hpp" #include +namespace detection_by_tracker +{ namespace utils { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -28,3 +30,4 @@ bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); } } // namespace utils +} // namespace detection_by_tracker diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index c021dd92dae64..b13f68b07181e 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/segmentation_pointcloud_fusion/node.cpp src/roi_pointcloud_fusion/node.cpp ) @@ -40,6 +41,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::SegmentPointCloudFusionNode" + EXECUTABLE segmentation_pointcloud_fusion_node +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" EXECUTABLE roi_pointcloud_fusion_node diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index e1be5426cba4b..21d31f216373b 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,18 +1,26 @@ /**: ros__parameters: - class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] - paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] - point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle - max_voxel_size: 40000 - point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] - voxel_size: [0.32, 0.32, 8.0] - downsample_factor: 1 - encoder_in_feature_size: 12 - yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] - # post-process params - circle_nms_dist_threshold: 0.3 - iou_nms_target_class_names: ["CAR"] - iou_nms_search_distance_2d: 10.0 - iou_nms_threshold: 0.1 - # omp params - omp_num_threads: 1 + model_params: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle + max_voxel_size: 40000 + point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + voxel_size: [0.32, 0.32, 8.0] + downsample_factor: 1 + encoder_in_feature_size: 12 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + has_twist: false + densification_params: + world_frame_id: "map" + num_past_frames: 0 + post_process_params: + # post-process params + circle_nms_dist_threshold: 0.3 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + score_threshold: 0.4 + omp_params: + # omp params + num_threads: 1 diff --git a/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml new file mode 100644 index 0000000000000..90ba841d53b2d --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_cluster_fusion.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + fusion_distance: 100.0 + trust_object_distance: 100.0 + trust_object_iou_mode: "iou" + non_trust_object_iou_mode: "iou_x" + use_cluster_semantic_type: false + only_allow_inside_cluster: true + roi_scale_factor: 1.1 + iou_threshold: 0.65 + unknown_iou_threshold: 0.1 + remove_unknown: true diff --git a/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..5b86b8e81d1aa --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_pointcloud_fusion.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + fuse_unknown_only: true + min_cluster_size: 2 + cluster_2d_tolerance: 0.5 diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml index 21ba13787f1c0..99d85089befb8 100644 --- a/perception/image_projection_based_fusion/config/roi_sync.param.yaml +++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml @@ -3,3 +3,11 @@ input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] timeout_ms: 70.0 match_threshold_ms: 50.0 + image_buffer_size: 15 + debug_mode: false + filter_scope_min_x: -100.0 + filter_scope_min_y: -100.0 + filter_scope_min_z: -100.0 + filter_scope_max_x: 100.0 + filter_scope_max_y: 100.0 + filter_scope_max_z: 100.0 diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000000..914ad13519807 --- /dev/null +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -0,0 +1,30 @@ +/**: + ros__parameters: + # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: + [ + true, # road + true, # sidewalk + true, # building + true, # wall + true, # fence + true, # pole + true, # traffic_light + true, # traffic_sign + true, # vegetation + true, # terrain + true, # sky + false, # person + false, # ride + false, # car + false, # truck + false, # bus + false, # train + false, # motorcycle + false, # bicycle + false, # others + ] + # the maximum distance of pointcloud to be applied filter, + # this is selected based on semantic segmentation model accuracy, + # calibration accuracy and unknown reaction distance + filter_distance_threshold: 60.0 diff --git a/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg b/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg new file mode 100644 index 0000000000000..aaadfaf186dd2 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion_pipeline.svg @@ -0,0 +1,966 @@ + + + + + + + + + + +
+
+
+ + Check whether + the cluster RoI (inner) + is fully contained within + the scaled detected RoI (outer) + . +
+
+ *NOTE* +
+ Because the cluster RoI is calculated with the points projected on the image, our recommended scale factor is [1.1, 1.5] . +
+ Otherwise, set + only_allow_inside_cluster + to + False + . +
+ +
+
+
+ + Check whether the cluster RoI (inner) is fully contained within the scaled... + + + + + + +
+
+
+ use_iou +
+
+
+ + use_iou + + + + + + +
+
+
+ unknown_iou_threshold +
+
+
+ + unknown_iou_threshold + + + + + + +
+
+
+ + use_iou_x /  y +
+ +
+
+
+ + use_iou_x /  y + + + + + + + + +
+
+
+ only_allow_inside_cluster +
+
+
+ + only_allow_inside_cluster + + + + + + +
+
+
+ iou_threshold +
+
+
+ + iou_threshold + + + + + + + + + + +
+
+
+ Is UNKNOWN object? +
+
+
+ + Is UNKNOWN object? + + + + + + + + +
+
+
+ + IoU threshold < IoU score? + +
+
+
+ + IoU threshold < IoU score? + + + + + + + + + + + + + + +
+
+
+ IoU Score +
+
+
+ + IoU Score + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Is Cluster RoI inside Detected RoI? +
+
+
+ + Is Cluster RoI inside Detected RoI? + + + + + + + + +
+
+
+ Fused Cluster +
+
+
+ + Fused Cluster + + + + + + + + +
+
+
+ Unfused Cluster +
+
+
+ + Unfused Cluster + + + + + + + + + + +
+
+
+ Fusion Target Cluster
&
Detected RoI +
+
+
+ + Fusion Target Cluster... + + + + + + +
+
+
+ Unfused Cluster +
+
+
+ + Unfused Cluster + + + + + + + + + + + + + +
+
+
+ + + roi_scale_factor
(>=1.0) + + +
+
+
+ + roi_scale_factor... + + + + + + +
+
+
+ + Check whether the IoU score between + the cluster RoI + and + the detected RoI + is greater than the threshold. +
+
+ **NOTE** +
+ Our default threshold is 0.65 with + only_allow_inside_cluster + is + True + . +
+ Also, please try the bigger score threshold and set + only_allow_inside_cluster + to + False + depending on your 2D RoI detector performance. +
+ +
+
+
+ + Check whether the IoU score between the cluster RoI and the detected RoI is greater than the thr... + + + + + + + + +
+
+
+
    +
  • + IoU = Intersection Area / Union Area +
    • +
  • + IoU X = Intersection Width / Union Width +
    • +
  • + IoU Y = Intersection Height / Union Height +
    • +
+
+
+
+ + IoU = Intersection Area / Union AreaIoU X = Intersectio... + + + + + + +
+
+
+ + Nodes overview + +
+
+
+ + Nodes overview + + + + + + +
+
+
+ Numeric Parameter +
+
+
+ + Numeric Parameter + + + + + + +
+
+
+ Result +
+
+
+ + Result + + + + + + + + + + +
+
+
+ Boolean Parameter +
+
+
+ + Boolean Parameter + + + + + + +
+
+
+ + true + +
+
+
+ + true + + + + + + +
+
+
+ + false + +
+
+
+ + false + + + + + + + + + + +
+
+
+ Condition +
+
+
+ + Condition + + + + + + +
+
+
+ + true + +
+
+
+ + true + + + + + + +
+
+
+ + false + +
+
+
+ + false + + + + + + +
+
+
+ use_cluster_semantic_type +
+
+
+ + use_cluster_semantic_type + + + + + + +
+
+
+ + Objects +
+ w/ Semantic Label + +
+
+
+ + Objects... + + + + + + + + +
+
+
+ + All objects are set to +
+ UNKOWN Label +
+ +
+
+
+ + All objects are set to... + + + + + + + + + + +
+
+
+ Fusion Target Cluster +
+
+
+ + Fusion Target Cluster + + + + + + +
+
+
+ trust_distance +
+
+
+ + trust_distance + + + + + + + + +
+
+
+ + Is Cluster closer than + trust_distance + ? + +
+
+
+ + Is Cluster closer than trust_... + + + + + + + + + + + + + + +
+
+
+ + Input Cluster + +
+
+
+ + Input Cluster + + + + + + +
+
+
+ roi_scale_factor +
+
+
+ + roi_scale_factor + + + + + + +
+
+
+ Pre-Process +
+
+
+ + Pre-Process + + + + + + +
+
+
+ Fusion Process +
+
+
+ + Fusion Process + + + + + + +
+
+
+ Post-Process +
+
+
+ + Post-Process + + + + + + + + +
+
+
+ remove_unknown +
+
+
+ + remove_unknown + + + + + + + + +
+
+
+ Fused Cluster +
+
+
+ + Fused Cluster + + + + + + +
+
+
+ Is KNOWN object?
&&
0.1 <= RoI probability?
 +
+
+
+ + Is KNOWN object?... + + + + + + + + +
+
+
+ Remove noise clusters, which are undetected by RoI detector, such as fog and exhaust gas. +
+
+
+ + Remove noise clusters, which are undet... + + + + + + +
+
+
+ + Output Fused Cluster + +
+
+
+ + Output Fused Cluster + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 03eaab2a3c6ca..86d3a2fa070b2 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -30,6 +30,11 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ## Parameters +The following figure is an inner pipeline overview of RoI cluster fusion node. +Please refer to it for your parameter settings. + +![roi_cluster_fusion_pipeline](./images/roi_cluster_fusion_pipeline.svg) + ### Core Parameters | Name | Type | Description | diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md new file mode 100644 index 0000000000000..77188aafb3b22 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -0,0 +1,87 @@ +# segmentation_pointcloud_fusion + +## Purpose + +The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud that are belong to less interesting region which is defined by semantic or instance segmentation by 2D image segmentation model. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto the semantic/instance segmentation mask image which is the output from 2D segmentation neural network. +- The pointclouds are belong to segment such as road, sidewalk, building, vegetation, sky ... are considered as less important points for autonomous driving and could be removed. + +![segmentation_pointcloud_fusion_image](./images/segmentation_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | ----------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| -------- | ------------------------------- | -------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output filtered pointcloud | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ------------- | ---- | ------------------------ | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 9572e62e0e0aa..fe8acb6746dba 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -49,13 +50,14 @@ namespace image_projection_based_fusion { using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using sensor_msgs::msg::CameraInfo; +using sensor_msgs::msg::Image; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; using autoware_auto_perception_msgs::msg::ObjectClassification; - -template +template class FusionNode : public rclcpp::Node { public: @@ -78,12 +80,12 @@ class FusionNode : public rclcpp::Node virtual void subCallback(const typename Msg::ConstSharedPtr input_msg); // callback for roi subscription + virtual void roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); virtual void fuseOnSingleImage( - const Msg & input_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, + const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0; // set args if you need @@ -111,7 +113,7 @@ class FusionNode : public rclcpp::Node /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; - std::vector::SharedPtr> rois_subs_; + std::vector::SharedPtr> rois_subs_; // offsets between cameras and the lidars std::vector input_offset_ms_; @@ -120,7 +122,7 @@ class FusionNode : public rclcpp::Node std::vector is_fused_; std::pair cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; + std::vector> cached_roi_msgs_; std::mutex mutex_cached_msgs_; // output publisher diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index d7de10fed068f..78ae152141a00 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -33,7 +33,8 @@ namespace image_projection_based_fusion { using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -class PointPaintingFusionNode : public FusionNode +class PointPaintingFusionNode +: public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp index c913ac33d5e84..897609fa3d86d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp @@ -27,8 +27,9 @@ cudaError_t generateVoxels_random_launch( cudaStream_t stream); cudaError_t generateBaseFeatures_launch( - unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, - float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream); + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size, + unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs, + cudaStream_t stream); cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index e54710ad477da..b9471f2f3b78e 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode -: public FusionNode +: public FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index e11a62c060480..7d7132309fb91 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -29,7 +29,8 @@ namespace image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode : public FusionNode +class RoiDetectedObjectFusionNode +: public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 2b4eb822a9edb..fe685baa0b6e2 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -22,7 +22,7 @@ namespace image_projection_based_fusion { class RoiPointCloudFusionNode -: public FusionNode +: public FusionNode { private: int min_cluster_size_{1}; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..4168c483469ab --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -0,0 +1,54 @@ +// 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 IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include + +#if __has_include() +#include +#else +#include +#endif + +namespace image_projection_based_fusion +{ +class SegmentPointCloudFusionNode : public FusionNode +{ +private: + rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + std::vector filter_semantic_label_target_; + float filter_distance_threshold_; + /* data */ +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(PointCloud2 & pointcloud_msg) override; + + void postprocess(PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, + const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + + bool out_of_scope(const PointCloud2 & filtered_cloud); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__SEGMENTATION_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index e15737f5ed222..33781461fa1cc 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -30,7 +30,6 @@ - @@ -43,9 +42,6 @@ - - - @@ -82,9 +78,6 @@ - - - diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 60f6f943b8cda..52dd71e9579c1 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -18,24 +18,11 @@ + - - - - - - - - - - - - - - @@ -46,17 +33,8 @@ - - - - - - - - - - + @@ -86,16 +64,6 @@ - - - - - - - - - - diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index b6165fc7c87d2..c9da81af9ddb0 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -25,8 +25,6 @@ - - @@ -69,10 +67,6 @@ - - - - diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index 181f4ccb88320..046d88d06e2a1 100644 --- a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -1,8 +1,5 @@ - - - @@ -23,6 +20,7 @@ + @@ -38,9 +36,7 @@ - - - + diff --git a/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..96bf47594bfe8 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/segmentation_pointcloud_fusion.launch.xml @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 1648de210ec2c..49ff4dafc7900 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -9,6 +9,8 @@ Shunsuke Miura Yoshi Ri badai nguyen + Kotaro Uetake + Tao Zhong Apache License 2.0 ament_cmake_auto diff --git a/perception/image_projection_based_fusion/schema/pointpainting.schema.json b/perception/image_projection_based_fusion/schema/pointpainting.schema.json new file mode 100644 index 0000000000000..036628d72e70a --- /dev/null +++ b/perception/image_projection_based_fusion/schema/pointpainting.schema.json @@ -0,0 +1,152 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Point Painting Fusion Node", + "type": "object", + "definitions": { + "pointpainting": { + "type": "object", + "properties": { + "model_params": { + "type": "object", + "description": "Parameters for model configuration.", + "properties": { + "class_names": { + "type": "array", + "description": "An array of class names will be predicted.", + "default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "paint_class_names": { + "type": "array", + "description": "An array of class names will be painted by PointPainting", + "default": ["CAR", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "point_feature_size": { + "type": "integer", + "description": "A number of channels of point feature layer.", + "default": 7 + }, + "max_voxel_size": { + "type": "integer", + "description": "A maximum size of voxel grid.", + "default": 40000 + }, + "point_cloud_range": { + "type": "array", + "description": "An array of distance ranges of each class, this must have same length with `class_names`.", + "default": [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + }, + "voxel_size": { + "type": "array", + "description": "An array of voxel grid sizes for PointPainting, this must have same length with `paint_class_names`.", + "default": [0.32, 0.32, 8.0] + }, + "down_sample_factor": { + "type": "integer", + "description": "A scale factor of downsampling points", + "default": 1, + "minimum": 1 + }, + "encoder_in_feature_size": { + "type": "integer", + "description": "A size of encoder input feature channels.", + "default": 12 + }, + "yaw_norm_thresholds": { + "type": "array", + "description": "An array of distance threshold values of norm of yaw [rad].", + "default": [0.3, 0.3, 0.3, 0.3, 0.0], + "minimum": 0.0, + "maximum": 1.0 + }, + "has_twist": { + "type": "boolean", + "description": "Indicates whether the model outputs twist value.", + "default": false + } + } + }, + "densification_params": { + "type": "object", + "description": "Parameters for pointcloud densification.", + "properties": { + "world_frame_id": { + "type": "string", + "description": "A name of frame id where world coordinates system is defined with respect to.", + "default": "map" + }, + "num_past_frames": { + "type": "integer", + "description": "A number of past frames to be considered as same input frame.", + "default": 0, + "minimum": 0 + } + } + }, + "post_process_params": { + "type": "object", + "properties": { + "score_threshold": { + "type": "number", + "description": "A threshold value of existence probability score, all of objects with score less than this threshold are ignored.", + "default": 0.4, + "minimum": 0.0, + "maximum": 1.0 + }, + "circle_nms_dist_threshold": { + "type": "number", + "description": "", + "default": 0.3, + "minimum": 0.0, + "maximum": 1.0 + }, + "iou_nms_target_class_names": { + "type": "array", + "description": "An array of class names to be target in NMS.", + "default": ["CAR"], + "uniqueItems": true + }, + "iou_search_distance_2d": { + "type": "number", + "description": "A maximum distance value to search the nearest objects.", + "default": 10.0, + "minimum": 0.0 + }, + "iou_nms_threshold": { + "type": "number", + "description": "A threshold value of NMS using IoU score.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + } + } + }, + "omp_params": { + "type": "object", + "properties": { + "num_threads": { + "type": "integer", + "description": "The number of threads that is set to the environment variable OMP_NUM_THREADS.", + "default": 1, + "minimum": 1 + } + } + } + }, + "required": ["model_params", "densification_params", "post_process_params", "omp_params"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pointpainting" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json new file mode 100644 index 0000000000000..fc32e9d6d3d8b --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_cluster_fusion.schema.json @@ -0,0 +1,96 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI Cluster Fusion Node", + "type": "object", + "definitions": { + "roi_cluster_fusion": { + "type": "object", + "properties": { + "fusion_distance": { + "type": "number", + "description": "If the detected object's distance is less than its value, the fusion will be processed.", + "default": 100.0, + "minimum": 0.0 + }, + "trust_object_distance": { + "type": "number", + "description": "If the detected object's distance is less than its value, IoU method specified in `trust_object_iou_mode` is used, otherwise `non_trust_object_iou_mode` is used.", + "default": 100.0, + "minimum": 0.0 + }, + "trust_object_iou_mode": { + "type": "string", + "description": "Name of IoU method applied to the objects in range of [0.0, `trust_distance`].", + "default": "iou", + "enum": ["iou", "iou_x", "iou_y"] + }, + "non_trust_object_iou_mode": { + "type": "string", + "description": "Name of IoU method applied to the objects in range of [`trust_distance`, `fusion_distance`], if `trust_distance` < `fusion_distance`.", + "default": "iou_x", + "enum": ["iou", "iou_x", "iou_y"] + }, + "use_cluster_semantic_type": { + "type": "boolean", + "description": "If this parameter is false, label of cluster objects will be reset to UNKNOWN.", + "default": false + }, + "only_allow_inside_cluster": { + "type": "boolean", + "description": "If this parameter is true, only clusters in which all their points are inside the RoI can be assigned to the RoI.", + "default": true + }, + "roi_scale_factor": { + "type": "number", + "description": "A scale factor for resizing RoI while checking if cluster points are inside the RoI.", + "default": 1.1, + "minimum": 1.0, + "maximum": 2.0 + }, + "iou_threshold": { + "type": "number", + "description": "An IoU score threshold. Note that the total IoU score is the sum of the IoU scores that are set to true in use_iou, use_iou_x and use_iou_y.", + "default": 0.65, + "minimum": 0.0, + "maximum": 1.0 + }, + "unknown_iou_threshold": { + "type": "number", + "description": "A threshold value of IoU score for objects labeled UNKNOWN.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + }, + "remove_unknown": { + "type": "boolean", + "description": "If this parameter is true, all of objects labeled UNKNOWN will be removed in post-process.", + "default": false + } + }, + "required": [ + "fusion_distance", + "trust_object_distance", + "trust_object_iou_mode", + "non_trust_object_iou_mode", + "use_cluster_semantic_type", + "only_allow_inside_cluster", + "roi_scale_factor", + "iou_threshold", + "unknown_iou_threshold", + "remove_unknown" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_cluster_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json new file mode 100644 index 0000000000000..3030be1305d56 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_detected_object_fusion.schema.json @@ -0,0 +1,70 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI Detected Object Fusion Node", + "type": "object", + "definitions": { + "roi_detected_object_fusion": { + "type": "object", + "properties": { + "passthrough_lower_bound_probability_thresholds": { + "type": "array", + "description": "An array of object probability thresholds. The objects that have higher probability than their respective thresholds are kept.", + "default": [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.5] + }, + "trust_distances": { + "type": "array", + "description": "An array of object distances thresholds. Any objects that is farther than this value will be skipped in the clustering process, but will still be published.", + "default": [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0] + }, + "min_iou_threshold": { + "type": "number", + "description": "An Iou score threshold.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + }, + "roi_probability_threshold": { + "type": "number", + "description": "A object probability threshold.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + }, + "use_roi_probability": { + "type": "boolean", + "description": "If this parameter is true, the objects are filtered out with their RoI probabilities.", + "default": false + }, + "can_assign_matrix": { + "type": "array", + "description": "An NxN matrix, where N represents the number of classes. A value 1 indicates that it is assignable, while a value of 0 indicates not.", + "default": [ + 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, + 0, 0, 0, 1, 1, 1 + ] + } + }, + "required": [ + "passthrough_lower_bound_probability_thresholds", + "trust_distances", + "min_iou_threshold", + "roi_probability_threshold", + "use_roi_probability", + "can_assign_matrix" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_detected_object_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json new file mode 100644 index 0000000000000..f39ef257ea789 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_pointcloud_fusion.schema.json @@ -0,0 +1,41 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for RoI PointCloud Fusion Node", + "type": "object", + "definitions": { + "roi_pointcloud_fusion": { + "type": "object", + "properties": { + "fuse_unknown_only": { + "type": "boolean", + "description": "Whether to fuse only UNKNOWN clusters.", + "default": true + }, + "min_cluster_size": { + "type": "integer", + "description": "The minimum number of points that a cluster must contain to be considered as valid.", + "default": 2 + }, + "cluster_2d_tolerance": { + "type": "number", + "description": "A cluster tolerance measured in radial direction [m]", + "default": 0.5, + "exclusiveMinimum": 0.0 + } + }, + "required": ["fuse_unknown_only", "min_cluster_size", "cluster_2d_tolerance"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_pointcloud_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/schema/roi_sync.schema.json b/perception/image_projection_based_fusion/schema/roi_sync.schema.json new file mode 100644 index 0000000000000..411fb678a49a7 --- /dev/null +++ b/perception/image_projection_based_fusion/schema/roi_sync.schema.json @@ -0,0 +1,84 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Synchronization of RoI Fusion Nodes", + "type": "object", + "definitions": { + "roi_sync": { + "type": "object", + "properties": { + "input_offset_ms": { + "type": "array", + "description": "An array of timestamp offsets for each camera [ms].", + "default": [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] + }, + "timeout_ms": { + "type": "number", + "description": "A timeout value can be assigned within a single frame [ms].", + "default": 70.0, + "minimum": 1.0, + "maximum": 100.0 + }, + "match_threshold_ms": { + "type": "number", + "description": "A maximum threshold value to synchronize RoIs from multiple cameras [ms].", + "default": 50.0, + "minimum": 0.0, + "maximum": 100.0 + }, + "image_buffer_size": { + "type": "integer", + "description": "The number of image buffer size for debug.", + "default": 15, + "minimum": 1 + }, + "debug_mode": { + "type": "boolean", + "description": "Whether to debug or not.", + "default": false + }, + "filter_scope_min_x": { + "type": "number", + "description": "Minimum x position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_min_y": { + "type": "number", + "description": "Minimum y position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_min_z": { + "type": "number", + "description": "Minimum z position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_max_x": { + "type": "number", + "description": "Maximum x position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_max_y": { + "type": "number", + "description": "Maximum y position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_max_z": { + "type": "number", + "description": "Maximum z position [m].", + "default": 100.0 + } + } + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_sync" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index b01a910aaded1..6bced39bc61ef 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -40,13 +40,13 @@ static double processing_time_ms = 0; namespace image_projection_based_fusion { -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number", 1)); + rois_number_ = static_cast(declare_parameter("rois_number")); if (rois_number_ < 1) { RCLCPP_WARN( this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); @@ -80,8 +80,8 @@ FusionNode::FusionNode( "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); } - input_offset_ms_ = declare_parameter("input_offset_ms", std::vector{}); - if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { + input_offset_ms_ = declare_parameter>("input_offset_ms"); + if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); } @@ -99,9 +99,9 @@ FusionNode::FusionNode( cached_roi_msgs_.resize(rois_number_); is_fused_.resize(rois_number_, false); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - std::function roi_callback = + std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); - rois_subs_.at(roi_i) = this->create_subscription( + rois_subs_.at(roi_i) = this->create_subscription( input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } @@ -122,7 +122,7 @@ FusionNode::FusionNode( // debugger if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = - static_cast(declare_parameter("image_buffer_size", 15)); + static_cast(declare_parameter("image_buffer_size")); debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } @@ -136,32 +136,33 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } + // cspell: ignore minx, maxx, miny, maxy, minz, maxz // FIXME: use min_x instead of minx - filter_scope_minx_ = declare_parameter("filter_scope_minx", -100); - filter_scope_maxx_ = declare_parameter("filter_scope_maxx", 100); - filter_scope_miny_ = declare_parameter("filter_scope_miny", -100); - filter_scope_maxy_ = declare_parameter("filter_scope_maxy", 100); - filter_scope_minz_ = declare_parameter("filter_scope_minz", -100); - filter_scope_maxz_ = declare_parameter("filter_scope_maxz", 100); + filter_scope_minx_ = declare_parameter("filter_scope_min_x"); + filter_scope_maxx_ = declare_parameter("filter_scope_max_x"); + filter_scope_miny_ = declare_parameter("filter_scope_min_y"); + filter_scope_maxy_ = declare_parameter("filter_scope_max_y"); + filter_scope_minz_ = declare_parameter("filter_scope_min_z"); + filter_scope_maxz_ = declare_parameter("filter_scope_max_z"); } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { camera_info_map_[camera_id] = *input_camera_info_msg; } -template -void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) +template +void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) +template +void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -284,9 +285,9 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } } -template -void FusionNode::roiCallback( - const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { stop_watch_ptr_->toc("processing_time", true); @@ -349,14 +350,14 @@ void FusionNode::roiCallback( (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; } -template -void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) +template +void FusionNode::postprocess(Msg & output_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { using std::chrono_literals::operator""ms; timer_->cancel(); @@ -394,8 +395,8 @@ void FusionNode::timer_callback() } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -411,8 +412,8 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const Msg & output_msg) +template +void FusionNode::publish(const Msg & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -420,8 +421,10 @@ void FusionNode::publish(const Msg & output_msg) pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode; -template class FusionNode; -template class FusionNode; +template class FusionNode; +template class FusionNode< + DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; +template class FusionNode; +template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 48ef3d26806c8..14783a46ba8b4 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -93,30 +93,32 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("pointpainting_fusion", options) +: FusionNode( + "pointpainting_fusion", options) { - omp_num_threads_ = this->declare_parameter("omp_num_threads", 1); + omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = - static_cast(this->declare_parameter("score_threshold", 0.4)); - const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + static_cast(this->declare_parameter("post_process_params.score_threshold")); + const float circle_nms_dist_threshold = static_cast( + this->declare_parameter("post_process_params.circle_nms_dist_threshold")); const auto yaw_norm_thresholds = - this->declare_parameter>("yaw_norm_thresholds"); + this->declare_parameter>("model_params.yaw_norm_thresholds"); // densification param const std::string densification_world_frame_id = - this->declare_parameter("densification_world_frame_id", "map"); + this->declare_parameter("densification_params.world_frame_id"); const int densification_num_past_frames = - this->declare_parameter("densification_num_past_frames", 0); + this->declare_parameter("densification_params.num_past_frames"); // network param - const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); - const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path", ""); - const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path", ""); - const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); - const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); - - class_names_ = this->declare_parameter>("class_names"); + const std::string trt_precision = this->declare_parameter("trt_precision"); + const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); + const std::string encoder_engine_path = + this->declare_parameter("encoder_engine_path"); + const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); + const std::string head_engine_path = this->declare_parameter("head_engine_path"); + + class_names_ = this->declare_parameter>("model_params.class_names"); const auto paint_class_names = - this->declare_parameter>("paint_class_names"); + this->declare_parameter>("model_params.paint_class_names"); std::vector classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"}; if ( std::find(paint_class_names.begin(), paint_class_names.end(), "TRUCK") != @@ -138,17 +140,17 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_.erase(cls); } } - has_twist_ = this->declare_parameter("has_twist", false); - const std::size_t point_feature_size = - static_cast(this->declare_parameter("point_feature_size")); + has_twist_ = this->declare_parameter("model_params.has_twist"); + const std::size_t point_feature_size = static_cast( + this->declare_parameter("model_params.point_feature_size")); const std::size_t max_voxel_size = - static_cast(this->declare_parameter("max_voxel_size")); - pointcloud_range = this->declare_parameter>("point_cloud_range"); - const auto voxel_size = this->declare_parameter>("voxel_size"); - const std::size_t downsample_factor = - static_cast(this->declare_parameter("downsample_factor")); - const std::size_t encoder_in_feature_size = - static_cast(this->declare_parameter("encoder_in_feature_size")); + static_cast(this->declare_parameter("model_params.max_voxel_size")); + pointcloud_range = this->declare_parameter>("model_params.point_cloud_range"); + const auto voxel_size = this->declare_parameter>("model_params.voxel_size"); + const std::size_t downsample_factor = static_cast( + this->declare_parameter("model_params.downsample_factor")); + const std::size_t encoder_in_feature_size = static_cast( + this->declare_parameter("model_params.encoder_in_feature_size")); const auto allow_remapping_by_area_matrix = this->declare_parameter>("allow_remapping_by_area_matrix"); const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); @@ -172,10 +174,11 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt { centerpoint::NMSParams p; p.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; - p.target_class_names_ = - this->declare_parameter>("iou_nms_target_class_names"); - p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); - p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + p.target_class_names_ = this->declare_parameter>( + "post_process_params.iou_nms_target_class_names"); + p.search_distance_2d_ = + this->declare_parameter("post_process_params.iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("post_process_params.iou_nms_threshold"); iou_bev_nms_.setParameters(p); } diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index 8911442f4c75d..d44620995c61b 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -69,8 +69,8 @@ bool PointPaintingTRT::preprocess( CHECK_CUDA_ERROR(image_projection_based_fusion::generateBaseFeatures_launch( mask_d_.get(), voxels_buffer_d_.get(), config_.grid_size_y_, config_.grid_size_x_, - num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), - stream_)); + config_.max_voxel_size_, num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), + coordinates_d_.get(), stream_)); CHECK_CUDA_ERROR(image_projection_based_fusion::generateFeatures_launch( voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index d06b60633acf8..68e08ac61a569 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -105,8 +105,8 @@ cudaError_t generateVoxels_random_launch( } __global__ void generateBaseFeatures_kernel( - unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, - float * voxel_features, float * voxel_num, int * voxel_idxs) + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size, + unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs) { unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x; unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y; @@ -120,6 +120,7 @@ __global__ void generateBaseFeatures_kernel( unsigned int current_pillarId = 0; current_pillarId = atomicAdd(pillar_num, 1); + if (current_pillarId > max_voxel_size - 1) return; voxel_num[current_pillarId] = count; @@ -140,15 +141,17 @@ __global__ void generateBaseFeatures_kernel( // create 4 channels cudaError_t generateBaseFeatures_launch( - unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, - float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream) + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size, + unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs, + cudaStream_t stream) { dim3 threads = {32, 32}; dim3 blocks = { (grid_x_size + threads.x - 1) / threads.x, (grid_y_size + threads.y - 1) / threads.y}; generateBaseFeatures_kernel<<>>( - mask, voxels, grid_y_size, grid_x_size, pillar_num, voxel_features, voxel_num, voxel_idxs); + mask, voxels, grid_y_size, grid_x_size, max_voxel_size, pillar_num, voxel_features, voxel_num, + voxel_idxs); cudaError_t err = cudaGetLastError(); return err; } diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 492f832e30e8b..1c064e4d0f060 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -34,7 +34,8 @@ namespace image_projection_based_fusion { RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_cluster_fusion", options) +: FusionNode( + "roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 333ec7d7ed206..19defc0a1cab0 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -25,7 +25,8 @@ namespace image_projection_based_fusion { RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_detected_object_fusion", options) +: FusionNode( + "roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index 233e568ebee0b..421aa3a453451 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -30,7 +30,8 @@ namespace image_projection_based_fusion { RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_pointcloud_fusion", options) +: FusionNode( + "roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..1c9c865b8d21e --- /dev/null +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -0,0 +1,139 @@ +// 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 "image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +namespace image_projection_based_fusion +{ +SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("segmentation_pointcloud_fusion", options) +{ + filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); + filter_semantic_label_target_ = + declare_parameter>("filter_semantic_label_target"); +} + +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} + +void SegmentPointCloudFusionNode::postprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +{ + return; +} +void SegmentPointCloudFusionNode::fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, + [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, + __attribute__((unused)) PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + cv_bridge::CvImagePtr in_image_ptr; + try { + in_image_ptr = cv_bridge::toCvCopy( + std::make_shared(input_mask), input_mask.encoding); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception:%s", e.what()); + return; + } + + cv::Mat mask = in_image_ptr->image; + if (mask.cols == 0 || mask.rows == 0) { + return; + } + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, + 0.0, 1.0; + geometry_msgs::msg::TransformStamped transform_stamped; + // transform pointcloud from frame id to camera optical frame id + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_mask.header.frame_id, input_pointcloud_msg.header.frame_id, + input_pointcloud_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + PointCloud output_cloud; + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + // skip filtering pointcloud behind the camera or too far from camera + if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + bool is_inside_image = + normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && + normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; + if (!is_inside_image) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + + // skip filtering pointcloud where semantic id out of the defined list + uint8_t semantic_id = mask.at( + static_cast(normalized_projected_point.y()), + static_cast(normalized_projected_point.x())); + if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + continue; + } + if (!filter_semantic_label_target_.at(semantic_id)) { + output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + + pcl::toROSMsg(output_cloud, output_pointcloud_msg); + output_pointcloud_msg.header = input_pointcloud_msg.header; +} + +bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const PointCloud2 & filtered_cloud) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::SegmentPointCloudFusionNode) diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp index 824144fe3b22b..9488b67475509 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp @@ -27,8 +27,9 @@ cudaError_t generateVoxels_random_launch( cudaStream_t stream); cudaError_t generateBaseFeatures_launch( - unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, - float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream); + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size, + unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs, + cudaStream_t stream); cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 67271985d3b5e..2804e172b73fa 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -156,8 +156,8 @@ bool CenterPointTRT::preprocess( CHECK_CUDA_ERROR(generateBaseFeatures_launch( mask_d_.get(), voxels_buffer_d_.get(), config_.grid_size_y_, config_.grid_size_x_, - num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), - stream_)); + config_.max_voxel_size_, num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), + coordinates_d_.get(), stream_)); CHECK_CUDA_ERROR(generateFeatures_launch( voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 6f77ff84c2cea..118e31f892d72 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -87,8 +87,8 @@ cudaError_t generateVoxels_random_launch( } __global__ void generateBaseFeatures_kernel( - unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, - float * voxel_features, float * voxel_num, int * voxel_idxs) + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size, + unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs) { unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x; unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y; @@ -102,6 +102,7 @@ __global__ void generateBaseFeatures_kernel( unsigned int current_pillarId = 0; current_pillarId = atomicAdd(pillar_num, 1); + if (current_pillarId > max_voxel_size - 1) return; voxel_num[current_pillarId] = count; @@ -120,15 +121,17 @@ __global__ void generateBaseFeatures_kernel( // create 4 channels cudaError_t generateBaseFeatures_launch( - unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, - float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream) + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size, + unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs, + cudaStream_t stream) { dim3 threads = {32, 32}; dim3 blocks = { (grid_x_size + threads.x - 1) / threads.x, (grid_y_size + threads.y - 1) / threads.y}; generateBaseFeatures_kernel<<>>( - mask, voxels, grid_y_size, grid_x_size, pillar_num, voxel_features, voxel_num, voxel_idxs); + mask, voxels, grid_y_size, grid_x_size, max_voxel_size, pillar_num, voxel_features, voxel_num, + voxel_idxs); cudaError_t err = cudaGetLastError(); return err; } diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 6d3d7f5e035a9..2419cdb010799 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -66,7 +66,7 @@ Lane change logics is illustrated in the figure below.An example of how to tune ### Tuning lane change detection logic -Currently we provide two parameters to tune lane change detection: +Currently we provide three parameters to tune lane change detection: - `dist_threshold_to_bound_`: maximum distance from lane boundary allowed for lane changing vehicle - `time_threshold_to_bound_`: maximum time allowed for lane change vehicle to reach the boundary @@ -124,6 +124,60 @@ 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 (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. + +Currently we provide three parameters to tune the lateral acceleration constraint: + +- `check_lateral_acceleration_constraints_`: to enable the constraint check. +- `max_lateral_accel_`: max acceptable lateral acceleration for predicted paths (absolute value). +- `min_acceleration_before_curve_`: the minimum acceleration the vehicle would theoretically use to slow down before a curve is taken (must be negative). + +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] | + +## 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 This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: 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 69ff96a263f4b..fdd64174de9be 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -13,6 +13,12 @@ sigma_yaw_angle_deg: 5.0 #[angle degree] object_buffer_time_length: 2.0 #[s] history_time_length: 1.0 #[s] + 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 bdd9ad89bc025..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 @@ -16,11 +16,16 @@ #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ #include "map_based_prediction/path_generator.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include #include +#include #include +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include #include #include @@ -34,6 +39,7 @@ #include #include +#include #include #include #include @@ -84,6 +90,7 @@ struct LaneletData struct PredictedRefPath { float probability; + double speed_limit; PosePath path; Maneuver maneuver; }; @@ -99,9 +106,10 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; - +using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node { public: @@ -123,6 +131,11 @@ class MapBasedPredictionNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; + // parameter update + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + // Pose Transform Listener tier4_autoware_utils::TransformListener transform_listener_{this}; @@ -160,6 +173,14 @@ class MapBasedPredictionNode : public rclcpp::Node bool consider_only_routable_neighbours_; double reference_path_resolution_; + bool check_lateral_acceleration_constraints_; + 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_; @@ -216,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( @@ -237,6 +259,115 @@ class MapBasedPredictionNode : public rclcpp::Node Maneuver predictObjectManeuverByLatDiffDistance( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); + + // NOTE: This function is copied from the motion_velocity_smoother package. + // TODO(someone): Consolidate functions and move them to a common + inline std::vector calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist) + { + using tier4_autoware_utils::calcCurvature; + using tier4_autoware_utils::getPoint; + + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } + + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } + + // calculate curvature by circle fitting from three points + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + try { + curvature = calcCurvature(p0, p1, p2); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN(rclcpp::get_logger("map_based_prediction"), "%s", e.what()); + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature + } else { + curvature = 0.0; + } + } + k_arr.at(i) = curvature; + } + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); + + return k_arr; + } + + inline TrajectoryPoints toTrajectoryPoints(const PredictedPath & path, const double velocity) + { + TrajectoryPoints out_trajectory; + std::for_each( + path.path.begin(), path.path.end(), [&out_trajectory, velocity](const auto & pose) { + TrajectoryPoint p; + p.pose = pose; + p.longitudinal_velocity_mps = velocity; + out_trajectory.push_back(p); + }); + return out_trajectory; + }; + + inline bool isLateralAccelerationConstraintSatisfied( + 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_); + + double arc_length = 0.0; + for (size_t i = 1; i < trajectory.size(); ++i) { + const auto current_pose = trajectory.at(i).pose; + const auto next_pose = trajectory.at(i - 1).pose; + // Compute distance between poses + const double delta_s = std::hypot( + next_pose.position.x - current_pose.position.x, + next_pose.position.y - current_pose.position.y); + arc_length += delta_s; + + // Compute change in heading + tf2::Quaternion q_current, q_next; + tf2::convert(current_pose.orientation, q_current); + tf2::convert(next_pose.orientation, q_next); + double delta_theta = q_current.angleShortestPath(q_next); + // Handle wrap-around + if (delta_theta > M_PI) { + delta_theta -= 2.0 * M_PI; + } else if (delta_theta < -M_PI) { + delta_theta += 2.0 * M_PI; + } + + 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; + }; }; } // namespace map_based_prediction 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/schema/map_based_pediction.schema.json b/perception/map_based_prediction/schema/map_based_pediction.schema.json new file mode 100644 index 0000000000000..8ddb122ebb56e --- /dev/null +++ b/perception/map_based_prediction/schema/map_based_pediction.schema.json @@ -0,0 +1,169 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Map Based Prediction", + "type": "object", + "definitions": { + "map_based_prediction": { + "type": "object", + "properties": { + "enable_delay_compensation": { + "type": "boolean", + "default": true, + "description": "flag to enable the time delay compensation for the position of the object" + }, + "prediction_time_horizon": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path" + }, + "lateral_control_time_horizon": { + "type": "number", + "default": "5.0", + "description": "time duration for predicted path will reach the reference path (mostly center of the lane)" + }, + "prediction_sampling_delta_time": { + "type": "number", + "default": "0.5", + "description": "sampling time for points in predicted path" + }, + "min_velocity_for_map_based_prediction": { + "type": "number", + "default": 1.39, + "description": "apply map-based prediction to the objects with higher velocity than this value" + }, + "min_crosswalk_user_velocity": { + "type": "number", + "default": 1.39, + "description": "minimum velocity use in path prediction for crosswalk users" + }, + "max_crosswalk_user_delta_yaw_threshold_for_lanelet": { + "type": "number", + "default": 0.785, + "description": "maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users" + }, + "dist_threshold_for_searching_lanelet": { + "type": "number", + "default": 3.0, + "description": "The threshold of the angle used when searching for the lane to which the object belongs " + }, + "delta_yaw_threshold_for_searching_lanelet": { + "type": "number", + "default": 0.785, + "description": "The threshold of the distance used when searching for the lane to which the object belongs" + }, + "sigma_lateral_offset": { + "type": "number", + "default": 0.5, + "description": "Standard deviation for lateral position of objects " + }, + "sigma_yaw_angle_deg": { + "type": "number", + "default": 5.0, + "description": "Standard deviation yaw angle of objects " + }, + "object_buffer_time_length": { + "type": "number", + "default": 2.0, + "description": "Time span of object history to store the information" + }, + "history_time_length": { + "type": "number", + "default": 1.0, + "description": "Time span of object information used for prediction" + }, + "prediction_time_horizon_rate_for_validate_shoulder_lane_length": { + "type": "number", + "default": 0.8, + "description": "prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length" + }, + "lane_change_detection": { + "type": "object", + "properties": { + "time_to_change_lane": { + "type": "object", + "properties": { + "dist_threshold_for_lane_change_detection": { + "type": "number", + "default": 1.0 + }, + "time_threshold_for_lane_change_detection": { + "type": "number", + "default": 5.0 + }, + "cutoff_freq_of_velocity_for_lane_change_detection": { + "type": "number", + "default": 0.1 + } + }, + "required": [ + "dist_threshold_for_lane_change_detection", + "time_threshold_for_lane_change_detection", + "cutoff_freq_of_velocity_for_lane_change_detection" + ] + }, + "lat_diff_distance": { + "type": "object", + "properties": { + "dist_ratio_threshold_to_left_bound": { + "type": "number", + "default": -0.5 + }, + "dist_ratio_threshold_to_right_bound": { + "type": "number", + "default": 0.5 + }, + "diff_dist_threshold_to_left_bound": { + "type": "number", + "default": 0.29 + }, + "diff_dist_threshold_to_right_bound": { + "type": "number", + "default": -0.29 + } + }, + "required": [ + "dist_ratio_threshold_to_left_bound", + "dist_ratio_threshold_to_right_bound", + "diff_dist_threshold_to_left_bound", + "diff_dist_threshold_to_right_bound" + ] + } + } + }, + "reference_path_resolution": { + "type": "number", + "default": 0.5, + "description": "Standard deviation for lateral position of objects " + } + }, + "required": [ + "enable_delay_compensation", + "prediction_time_horizon", + "lateral_control_time_horizon", + "prediction_sampling_delta_time", + "min_velocity_for_map_based_prediction", + "min_crosswalk_user_velocity", + "max_crosswalk_user_delta_yaw_threshold_for_lanelet", + "dist_threshold_for_searching_lanelet", + "delta_yaw_threshold_for_searching_lanelet", + "sigma_lateral_offset", + "sigma_yaw_angle_deg", + "object_buffer_time_length", + "history_time_length", + "prediction_time_horizon_rate_for_validate_shoulder_lane_length" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_based_prediction" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} 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 f89c83e9c2d5a..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] @@ -415,17 +410,21 @@ bool withinRoadLanelet( } boost::optional isReachableCrosswalkEdgePoints( - const TrackedObject & object, const CrosswalkEdgePoints & edge_points, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon, - const double min_object_vel) + const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk, + const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double time_horizon, const double min_object_vel) { using Point = boost::geometry::model::d2::point_xy; - using Line = boost::geometry::model::linestring; const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); + if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { + return {}; + } + const auto & p1 = edge_points.front_center_point; const auto & p2 = edge_points.back_center_point; @@ -442,62 +441,66 @@ boost::optional isReachableCrosswalkEdgePoints( const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet const auto surrounding_lanelets = lanelet::geometry::findNearest( - lanelet_map_ptr->laneletLayer, search_point, time_horizon * velocity); + lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity); - bool first_intersect_load = false; - bool second_intersect_load = false; - std::vector intersects_first; - std::vector intersects_second; - for (const auto & lanelet : surrounding_lanelets) { - if (withinLanelet(object, lanelet.second)) { - return {}; - } + const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) { + const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) { + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if ( + (attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) && + boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; - lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); - if (attr.value() != "road") { - continue; - } + const auto isExist = [](const Point & p, const std::vector & points) { + for (const auto & existingPoint : points) { + if (boost::geometry::distance(p, existingPoint) < 1e-1) { + return true; + } + } + return false; + }; - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}}; - std::vector tmp_intersects; - boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); - for (const auto & p : tmp_intersects) { - intersects_first.push_back(p); + std::vector points_of_intersect; + const boost::geometry::model::linestring line{p_src, p_dst}; + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != lanelet::AttributeValueString::Road) { + continue; } - } - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}}; std::vector tmp_intersects; boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); for (const auto & p : tmp_intersects) { - intersects_second.push_back(p); + if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { + continue; + } + points_of_intersect.push_back(p); + if (points_of_intersect.size() >= 2) { + return true; + } } } - } - - if (1 < intersects_first.size()) { - first_intersect_load = true; - } + return false; + }; - if (1 < intersects_second.size()) { - second_intersect_load = true; - } + const bool first_intersects_road = isAcrossAnyRoad( + {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}); + const bool second_intersects_road = + isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}); - if (first_intersect_load && second_intersect_load) { + if (first_intersects_road && second_intersects_road) { return {}; } - if (first_intersect_load && !second_intersect_load) { + if (first_intersects_road && !second_intersects_road) { ret.swap(); } @@ -605,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 @@ -740,6 +743,12 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg"); object_buffer_time_length_ = declare_parameter("object_buffer_time_length"); history_time_length_ = declare_parameter("history_time_length"); + + check_lateral_acceleration_constraints_ = + declare_parameter("check_lateral_acceleration_constraints"); + max_lateral_accel_ = declare_parameter("max_lateral_accel"); + min_acceleration_before_curve_ = declare_parameter("min_acceleration_before_curve"); + { // lane change detection lane_change_detection_method_ = declare_parameter("lane_change_detection.method"); @@ -774,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)); @@ -789,6 +806,32 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "max_lateral_accel", max_lateral_accel_); + 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; + result.reason = "success"; + return result; } PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics( @@ -972,16 +1015,53 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // Generate Predicted Path std::vector predicted_paths; + double min_avg_curvature = std::numeric_limits::max(); + PredictedPath path_with_smallest_avg_curvature; + for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path); - if (predicted_path.path.empty()) { + yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); + if (predicted_path.path.empty()) continue; + + if (!check_lateral_acceleration_constraints_) { + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); + continue; + } + + // Check lat. acceleration constraints + const auto trajectory_with_const_velocity = + toTrajectoryPoints(predicted_path, abs_obj_speed); + + if (isLateralAccelerationConstraintSatisfied( + trajectory_with_const_velocity, prediction_sampling_time_interval_)) { + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); + continue; + } + + // Calculate curvature assuming the trajectory points interval is constant + // In case all paths are deleted, a copy of the straightest path is kept + + constexpr double curvature_calculation_distance = 2.0; + constexpr double points_interval = 1.0; + const size_t idx_dist = static_cast( + std::max(static_cast((curvature_calculation_distance) / points_interval), 1)); + const auto curvature_v = + calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist); + if (curvature_v.empty()) { continue; } - predicted_path.confidence = ref_path.probability; - predicted_paths.push_back(predicted_path); + const auto curvature_avg = + std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size(); + if (curvature_avg < min_avg_curvature) { + min_avg_curvature = curvature_avg; + path_with_smallest_avg_curvature = predicted_path; + path_with_smallest_avg_curvature.confidence = ref_path.probability; + } } + if (predicted_paths.empty()) predicted_paths.push_back(path_with_smallest_avg_curvature); // Normalize Path Confidence and output the predicted object float sum_confidence = 0.0; @@ -1135,48 +1215,45 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( predicted_object.kinematics.predicted_paths.push_back(predicted_path); } } + } + // try to find the edge points for all crosswalks and generate path to the crosswalk edge + for (const auto & crosswalk : crosswalks_) { + const auto edge_points = getCrosswalkEdgePoints(crosswalk); + + const auto reachable_first = hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + const auto reachable_second = hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + + if (!reachable_first && !reachable_second) { + continue; + } - // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge - // points for all crosswalks and generate path to the crosswalk edge - } else { - for (const auto & crosswalk : crosswalks_) { - const auto edge_points = getCrosswalkEdgePoints(crosswalk); - - const auto reachable_first = hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - const auto reachable_second = hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - - if (!reachable_first && !reachable_second) { - continue; - } - - const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, edge_points, lanelet_map_ptr_, prediction_time_horizon_, - min_crosswalk_user_velocity_); - - if (!reachable_crosswalk) { - continue; - } + const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + min_crosswalk_user_velocity_); - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); - predicted_path.confidence = 1.0; + if (!reachable_crosswalk) { + continue; + } - if (predicted_path.path.empty()) { - continue; - } - // If the predicted path to the crosswalk is crossing the fence, don't use it - if (doesPathCrossAnyFence(predicted_path)) { - continue; - } + PredictedPath predicted_path = + path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); + if (predicted_path.path.empty()) { + continue; + } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; } + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); } const auto n_path = predicted_object.kinematics.predicted_paths.size(); @@ -1488,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 @@ -1577,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); @@ -1899,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); @@ -1909,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/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 3e379bcfd1cf1..f5fbc8ff950e8 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -12,6 +12,7 @@ endif() ### Find Eigen Dependencies find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +find_package(glog REQUIRED) include_directories( SYSTEM @@ -40,6 +41,7 @@ ament_auto_add_library(multi_object_tracker_node SHARED target_link_libraries(multi_object_tracker_node Eigen3::Eigen + glog::glog ) rclcpp_components_register_node(multi_object_tracker_node diff --git a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml index 9552b35b65bed..ca320c7f58442 100644 --- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml +++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -13,3 +13,9 @@ publish_rate: 10.0 world_frame_id: map enable_delay_compensation: false + + # debug parameters + publish_processing_time: false + publish_tentative_objects: false + diagnostics_warn_delay: 0.5 # [sec] + diagnostics_error_delay: 1.0 # [sec] diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index eb8607b91b50c..95d33b78ff184 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -23,6 +23,8 @@ #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include +#include +#include #include #include @@ -38,6 +40,9 @@ #include #endif +#include +#include + #include #include @@ -47,6 +52,41 @@ #include #include +/** + * @brief Debugger class for multi object tracker + * @details This class is used to publish debug information of multi object tracker + */ +class TrackerDebugger +{ +public: + explicit TrackerDebugger(rclcpp::Node & node); + void publishProcessingTime(); + void publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + void startStopWatch(); + void startMeasurementTime(const rclcpp::Time & measurement_header_stamp); + void setupDiagnostics(); + void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); + struct DEBUG_SETTINGS + { + bool publish_processing_time; + bool publish_tentative_objects; + double diagnostics_warn_delay; + double diagnostics_error_delay; + } debug_settings_; + double elapsed_time_from_sensor_input_ = 0.0; + diagnostic_updater::Updater diagnostic_updater_; + +private: + void loadParameters(); + rclcpp::Node & node_; + rclcpp::Publisher::SharedPtr + debug_tentative_objects_pub_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + rclcpp::Time last_input_stamp_; +}; + class MultiObjectTracker : public rclcpp::Node { public: @@ -59,6 +99,9 @@ class MultiObjectTracker : public rclcpp::Node detected_object_sub_; rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + // debugger class + std::unique_ptr debugger_; + tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -81,7 +124,7 @@ class MultiObjectTracker : public rclcpp::Node const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; - void publish(const rclcpp::Time & time) const; + void publish(const rclcpp::Time & time); inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index e3172dfd22349..763bd12fab79e 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -13,8 +13,10 @@ eigen3_cmake_module autoware_auto_perception_msgs + diagnostic_updater eigen kalman_filter + libgoogle-glog-dev mussp object_recognition_utils rclcpp diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index e42ff748c503e..1d4d50c7eab4c 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -16,6 +16,7 @@ #include +#include #include #include @@ -65,11 +66,122 @@ boost::optional getTransformAnonymous( } // namespace +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) +: diagnostic_updater_(&node), node_(node), last_input_stamp_(node.now()) +{ + // declare debug parameters to decide whether to publish debug topics + loadParameters(); + // initialize debug publishers + stop_watch_ptr_ = std::make_unique>(); + if (debug_settings_.publish_processing_time) { + processing_time_publisher_ = + std::make_unique(&node_, "multi_object_tracker"); + } + + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_ = + node_.create_publisher( + "debug/tentative_objects", rclcpp::QoS{1}); + } + + // initialize stop watch and diagnostics + startStopWatch(); + setupDiagnostics(); +} + +void TrackerDebugger::setupDiagnostics() +{ + diagnostic_updater_.setHardwareID(node_.get_name()); + diagnostic_updater_.add( + "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); + diagnostic_updater_.setPeriod(0.1); +} + +void TrackerDebugger::loadParameters() +{ + try { + debug_settings_.publish_processing_time = + node_.declare_parameter("publish_processing_time"); + debug_settings_.publish_tentative_objects = + node_.declare_parameter("publish_tentative_objects"); + debug_settings_.diagnostics_warn_delay = + node_.declare_parameter("diagnostics_warn_delay"); + debug_settings_.diagnostics_error_delay = + node_.declare_parameter("diagnostics_error_delay"); + } catch (const std::exception & e) { + RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); + debug_settings_.publish_processing_time = false; + debug_settings_.publish_tentative_objects = false; + debug_settings_.diagnostics_warn_delay = 0.5; + debug_settings_.diagnostics_error_delay = 1.0; + } +} + +void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + const double delay = elapsed_time_from_sensor_input_; // [s] + + if (delay == 0.0) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); + } else if (delay < debug_settings_.diagnostics_warn_delay) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is acceptable"); + } else if (delay < debug_settings_.diagnostics_error_delay) { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Detection delay is over warn threshold."); + } else { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Detection delay is over error threshold."); + } + + stat.add("Detection delay", delay); +} + +void TrackerDebugger::publishProcessingTime() +{ + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const auto current_time = node_.now(); + elapsed_time_from_sensor_input_ = (current_time - last_input_stamp_).seconds(); + if (debug_settings_.publish_processing_time) { + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + processing_time_publisher_->publish( + "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); + } +} + +void TrackerDebugger::publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_->publish(tentative_objects); + } +} + +void TrackerDebugger::startStopWatch() +{ + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); +} + +void TrackerDebugger::startMeasurementTime(const rclcpp::Time & measurement_header_stamp) +{ + last_input_stamp_ = measurement_header_stamp; + // start measuring processing time + stop_watch_ptr_->toc("processing_time", true); +} + MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("multi_object_tracker"); + google::InstallFailureSignalHandler(); + // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, @@ -82,6 +194,9 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + // Debug publishers + debugger_ = std::make_unique(*this); + auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_.setCreateTimerInterface(cti); @@ -126,6 +241,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) void MultiObjectTracker::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { + /* keep the latest input stamp and check transform*/ + debugger_->startMeasurementTime(rclcpp::Time(input_objects_msg->header.stamp)); const auto self_transform = getTransformAnonymous( tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); if (!self_transform) { @@ -324,7 +441,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish( return true; } -void MultiObjectTracker::publish(const rclcpp::Time & time) const +void MultiObjectTracker::publish(const rclcpp::Time & time) { const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count(); @@ -332,11 +449,16 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - autoware_auto_perception_msgs::msg::TrackedObjects output_msg; + autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; output_msg.header.stamp = time; + tentative_objects_msg.header = output_msg.header; + for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - if (!shouldTrackerPublish(*itr)) { + if (!shouldTrackerPublish(*itr)) { // for debug purpose + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + tentative_objects_msg.objects.push_back(object); continue; } autoware_auto_perception_msgs::msg::TrackedObject object; @@ -346,6 +468,10 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const // Publish tracked_objects_pub_->publish(output_msg); + + // Debugger Publish if enabled + debugger_->publishProcessingTime(); + debugger_->publishTentativeObjects(tentative_objects_msg); } RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp b/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp deleted file mode 100644 index 8432a4e870e20..0000000000000 --- a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver.hpp +++ /dev/null @@ -1,22 +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 OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ - -#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" -#include "object_association_merger/data_association/solver/mu_successive_shortest_path.hpp" -#include "object_association_merger/data_association/solver/successive_shortest_path.hpp" - -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/data_association.hpp b/perception/object_merger/include/object_merger/data_association/data_association.hpp similarity index 84% rename from perception/object_merger/include/object_association_merger/data_association/data_association.hpp rename to perception/object_merger/include/object_merger/data_association/data_association.hpp index c03dd40b0d3b7..4cd9302fb29db 100644 --- a/perception/object_merger/include/object_association_merger/data_association/data_association.hpp +++ b/perception/object_merger/include/object_merger/data_association/data_association.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ #include #include @@ -25,7 +25,7 @@ #include #define EIGEN_MPL2_ONLY -#include "object_association_merger/data_association/solver/gnn_solver.hpp" +#include "object_merger/data_association/solver/gnn_solver.hpp" #include #include @@ -56,4 +56,4 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..cead11d516421 --- /dev/null +++ b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// 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 OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "object_merger/data_association/solver/successive_shortest_path.hpp" + +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp similarity index 79% rename from perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp rename to perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp index ec4bc907d85f6..447f4fa2b7ce3 100644 --- a/perception/object_merger/include/object_association_merger/data_association/solver/gnn_solver_interface.hpp +++ b/perception/object_merger/include/object_merger/data_association/solver/gnn_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace gnn_solver -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp similarity index 72% rename from perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp rename to perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp index f9d88bd793db1..0464a29e6b2e5 100644 --- a/perception/object_merger/include/object_association_merger/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/object_merger/include/object_merger/data_association/solver/mu_successive_shortest_path.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" +#include "object_merger/data_association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace gnn_solver -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp b/perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp similarity index 72% rename from perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp rename to perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp index 2487faaa07c0e..618879a01866f 100644 --- a/perception/object_merger/include/object_association_merger/data_association/solver/successive_shortest_path.hpp +++ b/perception/object_merger/include/object_merger/data_association/solver/successive_shortest_path.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#include "object_association_merger/data_association/solver/gnn_solver_interface.hpp" +#include "object_merger/data_association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace gnn_solver -#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp similarity index 93% rename from perception/object_merger/include/object_association_merger/node.hpp rename to perception/object_merger/include/object_merger/node.hpp index 6815b59894083..8341ce490ca72 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_ASSOCIATION_MERGER__NODE_HPP_ -#define OBJECT_ASSOCIATION_MERGER__NODE_HPP_ +#ifndef OBJECT_MERGER__NODE_HPP_ +#define OBJECT_MERGER__NODE_HPP_ -#include "object_association_merger/data_association/data_association.hpp" +#include "object_merger/data_association/data_association.hpp" #include @@ -84,4 +84,4 @@ class ObjectAssociationMergerNode : public rclcpp::Node }; } // namespace object_association -#endif // OBJECT_ASSOCIATION_MERGER__NODE_HPP_ +#endif // OBJECT_MERGER__NODE_HPP_ diff --git a/perception/object_merger/include/object_association_merger/utils/utils.hpp b/perception/object_merger/include/object_merger/utils/utils.hpp similarity index 90% rename from perception/object_merger/include/object_association_merger/utils/utils.hpp rename to perception/object_merger/include/object_merger/utils/utils.hpp index bb4466bd4944d..40cade6cbbccb 100644 --- a/perception/object_merger/include/object_association_merger/utils/utils.hpp +++ b/perception/object_merger/include/object_merger/utils/utils.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ -#define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ +#ifndef OBJECT_MERGER__UTILS__UTILS_HPP_ +#define OBJECT_MERGER__UTILS__UTILS_HPP_ #include #include @@ -70,4 +70,4 @@ enum MSG_COV_IDX { }; } // namespace utils -#endif // OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ +#endif // OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp index 4eefcc1702c3b..4ccecbb9f60dd 100644 --- a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/data_association/data_association.hpp" +#include "object_merger/data_association/data_association.hpp" -#include "object_association_merger/data_association/solver/gnn_solver.hpp" -#include "object_association_merger/utils/utils.hpp" +#include "object_merger/data_association/solver/gnn_solver.hpp" +#include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp index bb3d1ed8fdaa4..e7fa56fee88c5 100644 --- a/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "object_merger/data_association/solver/mu_successive_shortest_path.hpp" #include diff --git a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp index 904d57b71f072..b38257d0188e0 100644 --- a/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/data_association/solver/successive_shortest_path.hpp" +#include "object_merger/data_association/solver/successive_shortest_path.hpp" #include #include diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index f776d2d940045..1ffc2791e8f4e 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_association_merger/node.hpp" +#include "object_merger/node.hpp" -#include "object_association_merger/utils/utils.hpp" +#include "object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 035845772ce53..928532f928e38 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -55,6 +55,24 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map EXECUTABLE laserscan_based_occupancy_grid_map_node ) +# GridMapFusionNode +ament_auto_add_library(synchronized_grid_map_fusion SHARED + src/fusion/synchronized_grid_map_fusion_node.cpp + src/fusion/single_frame_fusion_policy.cpp + src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp + src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp + src/utils/utils.cpp +) + +target_link_libraries(synchronized_grid_map_fusion + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(synchronized_grid_map_fusion + PLUGIN "synchronized_grid_map_fusion::GridMapFusionNode" + EXECUTABLE synchronized_grid_map_fusion_node +) + ament_auto_package( INSTALL_TO_SHARE launch @@ -66,13 +84,22 @@ if(BUILD_TESTING) # launch_testing find_package(launch_testing_ament_cmake REQUIRED) add_launch_test(test/test_pointcloud_based_method.py) + add_launch_test(test/test_synchronized_grid_map_fusion_node.py) # gtest ament_add_gtest(test_utils test/test_utils.cpp ) + ament_add_gtest(costmap_unit_tests + test/cost_value_test.cpp) + ament_add_gtest(fusion_policy_unit_tests + test/fusion_policy_test.cpp + src/fusion/single_frame_fusion_policy.cpp + ) target_link_libraries(test_utils ${PCL_LIBRARIES} ${PROJECT_NAME}_common ) + target_include_directories(costmap_unit_tests PRIVATE "include") + target_include_directories(fusion_policy_unit_tests PRIVATE "include") endif() diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 962bcdc6f154f..084c55d80d629 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -9,6 +9,7 @@ This package outputs the probability of having an obstacle as occupancy grid map - [Pointcloud based occupancy grid map](pointcloud-based-occupancy-grid-map.md) - [Laserscan based occupancy grid map](laserscan-based-occupancy-grid-map.md) +- [Grid map fusion](synchronized_grid_map_fusion.md) ## Settings @@ -70,3 +71,19 @@ Additional argument is shown below: | `container_name` | `occupancy_grid_map_container` | | | `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud | | `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud | + +### Test + +This package provides unit tests using `gtest`. +You can run the test by the following command. + +```bash +colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+ +``` + +Test contains the following. + +- Unit test for cost value conversion function +- Unit test for utility functions +- Unit test for occupancy grid map fusion functions +- Input/Output test for pointcloud based occupancy grid map diff --git a/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml new file mode 100644 index 0000000000000..f8a2dc2fbc7de --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/config/synchronized_grid_map_fusion_node.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # 1. fusion parameters + fusion_input_ogm_topics: ["topic1", "topic2"] + input_ogm_reliabilities: [0.8, 0.2] + fusion_method: "overwrite" # choose from ["overwrite", "log-odds", "dempster-shafer"] + + # 2. synchronization settings + match_threshold_sec: 0.01 # 10ms + timeout_sec: 0.1 # 100ms + input_offset_sec: [0.0, 0.0] # no offset + + # 3. settings for fused fusion map + # remember resolution and map size should be same with input maps + map_frame_: "map" + base_link_frame_: "base_link" + grid_map_origin_frame_: "base_link" + fusion_map_length_x: 100.0 + fusion_map_length_y: 100.0 + fusion_map_resolution: 0.5 diff --git a/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg new file mode 100644 index 0000000000000..6bf5ed98e4edf --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/synchronized_grid_map_fusion.drawio.svg @@ -0,0 +1,290 @@ + + + + + + + + + + +
+
+
+ OGM1 +
+
+
+ + OGM1 + + + + + + + + +
+
+
+ OGM2 +
+
+
+ + OGM2 + + + + + + + + +
+
+
+ OGM3 +
+
+
+ + OGM3 + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+ (Single Frame) +
+
+
+ + Fused... + + + + + + + + +
+
+
+ + Single Frame +
+ Fusion + +
+
+
+ + Single Frame... + + + + + + + + +
+
+
+ + Multi Frame +
+ Fusion + +
+
+
+ + Multi Frame... + + + + + + + + +
+
+
+ Fused +
+ OGM +
+
+
+ + Fused... + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
`t=t_{n-1}`
+
+
+ + `t=t_{n-1}` + + + + + + +
+
+
`t=t_n`
+
+
+ + `t=t_n` + + + + + + +
+
+
+ Publish +
+
+
+ + Publish + + + + + + +
+
+
+ input topics +
+
+
+ + input topics + + + + + + +
+
+
+ output topics +
+
+
+ + output topics + + + + + + +
+
+
+ grid_map_fusion::GridMapFusionNode +
+
+
+ + grid_map_fusion::GridMapFusionNode + + + + + + + Text is not SVG - cannot display + + + 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 73% 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 e9667ccf65577..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 @@ -59,19 +59,48 @@ static const unsigned char NO_INFORMATION = 128; // 0.5 * 255 static const unsigned char LETHAL_OBSTACLE = 255; static const unsigned char FREE_SPACE = 0; +static const unsigned char OCCUPIED_THRESHOLD = 180; +static const unsigned char FREE_THRESHOLD = 50; + struct CostTranslationTable { CostTranslationTable() { for (int i = 0; i < 256; i++) { - const auto value = static_cast(static_cast(i) * 100.f / 255.f); - data[i] = std::max(std::min(value, static_cast(99)), static_cast(1)); + const auto value = + static_cast(static_cast(i) * 100.f / 255.f); // 0-255 to 0-100 + data[i] = + std::max(std::min(value, static_cast(99)), static_cast(1)); // 0-100 to 1-99 } } char operator[](unsigned char n) const { return data[n]; } char data[256]; }; +struct InverseCostTranslationTable +{ + InverseCostTranslationTable() + { + // 0-100 to 0-255 + for (int i = 0; i < 100; i++) { + data[i] = static_cast(i * 255 / 99); + } + } + unsigned char operator[](char n) const + { + if (n > 99) { + return data[99]; + } else if (n < 1) { + return data[1]; + } else { + const unsigned char u_n = static_cast(n); + return data[u_n]; + } + } + unsigned char data[100]; +}; + 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/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp new file mode 100644 index 0000000000000..1f0553878ef5a --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp @@ -0,0 +1,66 @@ +// 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 PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ + +#include "probabilistic_occupancy_grid_map/cost_value.hpp" + +#include +#include +#include +#include + +/*min and max prob threshold to prevent log-odds to diverge*/ +#define EPSILON_PROB 0.03 + +namespace fusion_policy +{ +enum class FusionMethod { OVERWRITE, LOG_ODDS, DEMPSTER_SHAFER }; + +unsigned char convertProbabilityToChar(const double & probability); +double convertCharToProbability(const unsigned char & occupancy); +std::vector convertProbabilityToChar(const std::vector & probabilities); +std::vector convertCharToProbability(const std::vector & occupancies); + +namespace overwrite_fusion +{ +enum State : unsigned char { UNKNOWN = 0U, FREE = 1U, OCCUPIED = 2U }; +State getApproximateState(const unsigned char & occupancy); +unsigned char overwriteFusion(const std::vector & occupancies); +} // namespace overwrite_fusion + +namespace log_odds_fusion +{ +double logOddsFusion(const std::vector & probabilities); +double logOddsFusion( + const std::vector & probabilities, const std::vector & weights); +} // namespace log_odds_fusion + +namespace dempster_shafer_fusion +{ +struct dempsterShaferOccupancy; +double dempsterShaferFusion(const std::vector & probability); +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability); +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method); +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability); +} // namespace fusion_policy + +#endif // PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SINGLE_FRAME_FUSION_POLICY_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp new file mode 100644 index 0000000000000..8f09764688055 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -0,0 +1,127 @@ +// 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 PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ +#define PROBABILISTIC_OCCUPANCY_GRID_MAP__FUSION__SYNCHRONIZED_GRID_MAP_FUSION_NODE_HPP_ + +#include "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" +#include "probabilistic_occupancy_grid_map/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" +#include "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_updater_interface.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace synchronized_grid_map_fusion +{ + +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using costmap_2d::OccupancyGridMapUpdaterInterface; +using geometry_msgs::msg::Pose; + +class GridMapFusionNode : public rclcpp::Node +{ +public: + explicit GridMapFusionNode(const rclcpp::NodeOptions & node_options); + +private: + void onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & input_topic); + nav_msgs::msg::OccupancyGrid::UniquePtr OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map); + + nav2_costmap_2d::Costmap2D OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); + OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights); + + void updateGridMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg); + + void setPeriod(const int64_t new_period); + void timer_callback(); + void publish(); + +private: + // Publisher and Subscribers + rclcpp::Publisher::SharedPtr fused_map_pub_; + rclcpp::Publisher::SharedPtr single_frame_pub_; + std::vector::SharedPtr> grid_map_subs_; + std::unique_ptr> stop_watch_ptr_{}; + std::unique_ptr debug_publisher_ptr_{}; + + // Topics manager + std::size_t num_input_topics_{1}; + std::vector input_topics_; + std::vector input_offset_sec_; + std::vector input_topic_weights_; + std::map input_topic_weights_map_; + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // Timer to manage the timeout of the occupancy grid map + rclcpp::TimerBase::SharedPtr timer_; + double timeout_sec_{}; + double match_threshold_sec_{}; + + // cache for fusion + std::mutex mutex_; + std::shared_ptr + occupancy_grid_map_updater_ptr_; // contains fused grid map + std::map + gridmap_dict_; // temporary cache for grid map message + std::map + gridmap_dict_tmp_; // second cache for grid map message + std::map offset_map_; // time offset for each grid map + + // grid map parameters + std::string map_frame_; + std::string base_link_frame_; + std::string gridmap_origin_frame_; + float fusion_map_length_x_; + float fusion_map_length_y_; + float fusion_map_resolution_; + fusion_policy::FusionMethod fusion_method_; +}; + +} // namespace synchronized_grid_map_fusion + +#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/probabilistic_occupancy_grid_map/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 new file mode 100644 index 0000000000000..f9f100f285d38 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp @@ -0,0 +1,48 @@ +// 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 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 "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 + +// LOBF means: Log Odds Bayes Filter + +namespace costmap_2d +{ +class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface +{ +public: + enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + OccupancyGridMapLOBFUpdater( + const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) + : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + { + } + bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + void initRosParam(rclcpp::Node & node) override; + +private: + inline unsigned char applyLOBF(const unsigned char & z, const unsigned char & o); + Eigen::Matrix2f probability_matrix_; +}; + +} // namespace costmap_2d + +#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 91% 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 047b747c2861f..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,8 +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 "probabilistic_occupancy_grid_map/cost_value.hpp" #include #include @@ -114,6 +116,7 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); +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/launch/synchronized_occupancy_grid_map_fusion.launch.xml b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml new file mode 100644 index 0000000000000..c2391141e9fa0 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/launch/synchronized_occupancy_grid_map_fusion.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + 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 new file mode 100644 index 0000000000000..6ac681eff4916 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/single_frame_fusion_policy.cpp @@ -0,0 +1,322 @@ +// 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 "probabilistic_occupancy_grid_map/fusion/single_frame_fusion_policy.hpp" + +namespace fusion_policy +{ + +//// utils //// + +// convert [0, 1] to [0, 255] +unsigned char convertProbabilityToChar(const double & probability) +{ + return static_cast(probability * 255); +} +// convert [0, 255] to [0, 1] +double convertCharToProbability(const unsigned char & occupancy) +{ + return static_cast(occupancy) / 255.0; +} + +// convert [0, 1] to [0, 255] vectors +std::vector convertProbabilityToChar(const std::vector & probabilities) +{ + std::vector occupancies; + for (auto & probability : probabilities) { + occupancies.push_back(convertProbabilityToChar(probability)); + } + return occupancies; +} +// convert [0, 255] to [0, 1] vectors +std::vector convertCharToProbability(const std::vector & occupancies) +{ + std::vector probabilities; + for (auto & occupancy : occupancies) { + probabilities.push_back(convertCharToProbability(occupancy)); + } + return probabilities; +} + +/// @brief fusion with overwrite policy +namespace overwrite_fusion +{ + +/** + * @brief convert char value to occupancy state + * + * @param occupancy [0, 255] + * @return State + */ +State getApproximateState(const unsigned char & occupancy) +{ + if (occupancy >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return State::OCCUPIED; + } else if (occupancy <= occupancy_cost_value::FREE_THRESHOLD) { + return State::FREE; + } else { + return State::UNKNOWN; + } +} + +/** + * @brief override fusion + * + * @param occupancies : occupancies to be fused + * @return unsigned char + */ +unsigned char overwriteFusion(const std::vector & occupancies) +{ + if (occupancies.size() == 0) { + throw std::runtime_error("occupancies size is 0"); + } else if (occupancies.size() == 1) { + return occupancies[0]; + } + + auto fusion_occupancy = 128; // unknown + auto fusion_state = getApproximateState(fusion_occupancy); + for (auto & cell : occupancies) { + auto state = getApproximateState(cell); + if (state > fusion_state) { + fusion_state = state; + fusion_occupancy = cell; + } else if (state < fusion_state) { + continue; + } else { + fusion_occupancy = (fusion_occupancy / 2 + cell / 2); + } + } + return fusion_occupancy; +} +} // namespace overwrite_fusion + +/// @brief fusion with log-odds policy +namespace log_odds_fusion +{ +/** + * @brief log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @return double + */ +double logOddsFusion(const std::vector & probabilities) +{ + double log_odds = 0.0; + for (auto & probability : probabilities) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probability)); + log_odds += std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} + +/** + * @brief weighted log-odds fusion + * + * @param probabilities : probabilities of occupancy [0, 1] + * @param weights : weights of probabilities + * @return double + */ +double logOddsFusion(const std::vector & probabilities, const std::vector & weights) +{ + // check if the size of probabilities and weights are the same + if (probabilities.size() != weights.size()) { + // warning and return normal log-odds fusion + std::cout + << "The size of probabilities and weights are not the same. Return normal log-odds fusion." + << std::endl; + return logOddsFusion(probabilities); + } + + double log_odds = 0.0; + for (size_t i = 0; i < probabilities.size(); i++) { + const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probabilities[i])); + log_odds += weights[i] * std::log(p / (1.0 - p)); + } + return 1.0 / (1.0 + std::exp(-log_odds)); +} +} // namespace log_odds_fusion + +/// @brief fusion with Dempster-Shafer Theory +namespace dempster_shafer_fusion +{ +/** + * @brief conflict modified Dempster-Shafer Theory Data structure + * see https://www.diva-portal.org/smash/get/diva2:852457/FULLTEXT01.pdf + * + */ +struct dempsterShaferOccupancy +{ + double occupied; + double empty; + double unknown; + double conflict_threshold = 0.9; + + // initialize without args + dempsterShaferOccupancy() + { + occupied = 0.0; + empty = 0.0; + unknown = 1.0; + } + + // initialize with probability + dempsterShaferOccupancy(double occupied_probability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p; + empty = 1.0 - p; + unknown = 0.0; + } + + // initialize with probability and reliability + dempsterShaferOccupancy(double occupied_probability, double reliability) + { + // confine to [0, 1] + double p = std::max(0.0, std::min(1.0, occupied_probability)); + occupied = p * reliability; + empty = (1.0 - p) * reliability; + unknown = 1.0 - occupied - empty; + } + + // normalize + void normalize() + { + double sum = occupied + empty + unknown; + occupied /= sum; + empty /= sum; + unknown /= sum; + } + + // calc conflict factor K + double calcK(const dempsterShaferOccupancy & other) + { + return (occupied * other.empty + empty * other.occupied); + } + // calc sum of occupied probability mass + double calcOccupied(const dempsterShaferOccupancy & other) + { + return occupied * other.occupied + occupied * other.unknown + unknown * other.occupied; + } + // calc sum of empty probability mass + double calcEmpty(const dempsterShaferOccupancy & other) + { + return empty * other.empty + empty * other.unknown + unknown * other.empty; + } + + // Dempster-Shafer fusion + dempsterShaferOccupancy operator+(const dempsterShaferOccupancy & other) + { + dempsterShaferOccupancy result; + double K = calcK(other); + double O = calcOccupied(other); + double E = calcEmpty(other); + + if (K > conflict_threshold) { + // highly conflict + result.occupied = O; + result.empty = E; + result.unknown = 1 - O - E; + } else { + // low conflict + result.occupied = O / (1.0 - K); + result.empty = E / (1.0 - K); + result.unknown = 1 - result.occupied - result.empty; + } + return result; + } + + // get occupancy probability via Pignistic Probability + double getPignisticProbability() { return occupied + unknown / 2.0; } +}; + +/** + * @brief Dempster-Shafer fusion + * + * @param probability + * @return double + */ +double dempsterShaferFusion(const std::vector & probability) +{ + dempsterShaferOccupancy result; // init with unknown + for (auto & p : probability) { + result = result + dempsterShaferOccupancy(p); + } + return result.getPignisticProbability(); +} + +/** + * @brief Dempster-Shafer fusion with reliability + * + * @param probability + * @param reliability + * @return double + */ +double dempsterShaferFusion( + const std::vector & probability, const std::vector & reliability) +{ + // check if the size of probabilities and weights are the same + if (probability.size() != reliability.size()) { + // warning and return normal dempster-shafer fusion probability + std::cout << "The size of probabilities and reliability are not the same. Return normal " + "dempster-shafer fusion." + << std::endl; + return dempsterShaferFusion(probability); + } + + dempsterShaferOccupancy result; // init with unknown + for (size_t i = 0; i < probability.size(); i++) { + result = result + dempsterShaferOccupancy(probability[i], reliability[i]); + } + return result.getPignisticProbability(); +} +} // namespace dempster_shafer_fusion + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(dempster_shafer_fusion::dempsterShaferFusion(probability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +unsigned char singleFrameOccupancyFusion( + const std::vector & occupancy, FusionMethod method, + const std::vector & reliability) +{ + if (method == FusionMethod::OVERWRITE) { + return overwrite_fusion::overwriteFusion(occupancy); + } else if (method == FusionMethod::LOG_ODDS) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar(log_odds_fusion::logOddsFusion(probability, reliability)); + } else if (method == FusionMethod::DEMPSTER_SHAFER) { + auto probability = convertCharToProbability(occupancy); + return convertProbabilityToChar( + dempster_shafer_fusion::dempsterShaferFusion(probability, reliability)); + } else { + std::cout << "Unknown fusion method: " << std::endl; + return 128; + } +} + +} // 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 new file mode 100644 index 0000000000000..a88e65e712ac1 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -0,0 +1,458 @@ +// 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 "probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp" + +#include "probabilistic_occupancy_grid_map/cost_value.hpp" +#include "probabilistic_occupancy_grid_map/utils/utils.hpp" + +namespace synchronized_grid_map_fusion +{ +using costmap_2d::OccupancyGridMapFixedBlindSpot; +using costmap_2d::OccupancyGridMapLOBFUpdater; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; + +GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) +: Node("synchronized_occupancy_grid_map_fusion", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + /* load input parameters */ + { + // get input topics + declare_parameter("fusion_input_ogm_topics", std::vector{}); + input_topics_ = get_parameter("fusion_input_ogm_topics").as_string_array(); + if (input_topics_.empty()) { + throw std::runtime_error("The number of input topics must larger than 0."); + } + num_input_topics_ = input_topics_.size(); + if (num_input_topics_ < 1) { + RCLCPP_WARN( + this->get_logger(), "minimum num_input_topics_ is 1. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 1; + } + if (num_input_topics_ > 12) { + RCLCPP_WARN( + this->get_logger(), "maximum num_input_topics_ is 12. current num_input_topics_ is %zu", + num_input_topics_); + num_input_topics_ = 12; + } + // get input topic reliabilities + declare_parameter("input_ogm_reliabilities", std::vector{}); + input_topic_weights_ = get_parameter("input_ogm_reliabilities").as_double_array(); + + if (input_topic_weights_.empty()) { // no topic weight is set + // set equal weight + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = 1.0 / num_input_topics_; + } + } else if (num_input_topics_ == input_topic_weights_.size()) { + // set weight to map + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_topic_weights_map_[input_topics_.at(topic_i)] = input_topic_weights_.at(topic_i); + } + } else { + throw std::runtime_error("The number of weights does not match the number of topics."); + } + } + + // Set fusion timing parameters + match_threshold_sec_ = declare_parameter("match_threshold_sec", 0.01); + timeout_sec_ = declare_parameter("timeout_sec", 0.1); + input_offset_sec_ = declare_parameter("input_offset_sec", std::vector{}); + if (!input_offset_sec_.empty() && num_input_topics_ != input_offset_sec_.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } else if (input_offset_sec_.empty()) { + // if there are not input offset, set 0.0 + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + input_offset_sec_.push_back(0.0); + } + } + // Set fusion map parameters + map_frame_ = declare_parameter("map_frame_", "map"); + base_link_frame_ = declare_parameter("base_link_frame_", "base_link"); + gridmap_origin_frame_ = declare_parameter("grid_map_origin_frame_", "base_link"); + fusion_map_length_x_ = declare_parameter("fusion_map_length_x", 100.0); + fusion_map_length_y_ = declare_parameter("fusion_map_length_y", 100.0); + fusion_map_resolution_ = declare_parameter("fusion_map_resolution", 0.5); + + // set fusion method + std::string fusion_method_str = declare_parameter("fusion_method", "overwrite"); + if (fusion_method_str == "overwrite") { + fusion_method_ = fusion_policy::FusionMethod::OVERWRITE; + } else if (fusion_method_str == "log-odds") { + fusion_method_ = fusion_policy::FusionMethod::LOG_ODDS; + } else if (fusion_method_str == "dempster-shafer") { + fusion_method_ = fusion_policy::FusionMethod::DEMPSTER_SHAFER; + } else { + throw std::runtime_error("The fusion method is not supported."); + } + + // sub grid_map + grid_map_subs_.resize(num_input_topics_); + for (std::size_t topic_i = 0; topic_i < num_input_topics_; ++topic_i) { + std::function fnc = std::bind( + &GridMapFusionNode::onGridMap, this, std::placeholders::_1, input_topics_.at(topic_i)); + grid_map_subs_.at(topic_i) = this->create_subscription( + input_topics_.at(topic_i), rclcpp::QoS{1}.best_effort(), fnc); + } + + // pub grid_map + fused_map_pub_ = this->create_publisher( + "~/output/occupancy_grid_map", rclcpp::QoS{1}.reliable()); + single_frame_pub_ = this->create_publisher( + "~/debug/single_frame_map", rclcpp::QoS{1}.reliable()); + + // updater + occupancy_grid_map_updater_ptr_ = std::make_shared( + fusion_map_length_x_ / fusion_map_resolution_, fusion_map_length_y_ / fusion_map_resolution_, + fusion_map_resolution_); + + // Set timer + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&GridMapFusionNode::timer_callback, this)); + + // set offset map + for (size_t i = 0; i < input_offset_sec_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_sec_[i]; + } + + // init dict + for (size_t i = 0; i < input_topics_.size(); ++i) { + gridmap_dict_[input_topics_[i]] = nullptr; + gridmap_dict_tmp_[input_topics_[i]] = nullptr; + } + + // debug tools + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, "synchronized_grid_map_fusion"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } +} + +/** + * @brief Set the period of the timer + * + * @param new_period + */ +void GridMapFusionNode::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +/** + * @brief Callback function for the grid map message add data to buffer + * + * @param occupancy_grid_msg + * @param topic_name + */ +void GridMapFusionNode::onGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg, + const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + const bool is_already_subscribed_this = (gridmap_dict_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + // already subscribed + if (is_already_subscribed_this) { + gridmap_dict_tmp_[topic_name] = occupancy_grid_msg; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + // first time to subscribe in this period + } else { + gridmap_dict_[topic_name] = occupancy_grid_msg; // add to buffer + + // check if all topics are subscribed + bool is_subscribed_all = std::all_of( + std::begin(gridmap_dict_), std::end(gridmap_dict_), + [](const auto & e) { return e.second != nullptr; }); + // Then, go to publish + if (is_subscribed_all) { + for (const auto & e : gridmap_dict_tmp_) { + if (e.second != nullptr) { + gridmap_dict_[e.first] = e.second; + } + } + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + // if not all topics are subscribed, set timer + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +/** + * @brief Timer callback function + * + */ +void GridMapFusionNode::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +/** + * @brief Publisher function + * + */ +void GridMapFusionNode::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + builtin_interfaces::msg::Time latest_stamp = get_clock()->now(); + double height = 0.0; + + // merge available gridmap + std::vector subscribed_maps; + std::vector weights; + for (const auto & e : gridmap_dict_) { + if (e.second != nullptr) { + subscribed_maps.push_back(GridMapFusionNode::OccupancyGridMsgToGridMap(*e.second)); + weights.push_back(input_topic_weights_map_[e.first]); + latest_stamp = e.second->header.stamp; + height = e.second->info.origin.position.z; + } + } + + // return if empty + if (subscribed_maps.empty()) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "No grid map is subscribed. Please check the topic name and the topic type."); + return; + } + + // fusion map + // single frame gridmap fusion + auto fused_map = SingleFrameOccupancyFusion(subscribed_maps, latest_stamp, weights); + // multi frame gridmap fusion + occupancy_grid_map_updater_ptr_->update(fused_map); + + // publish + fused_map_pub_->publish( + OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, *occupancy_grid_map_updater_ptr_)); + single_frame_pub_->publish(OccupancyGridMapToMsgPtr(map_frame_, latest_stamp, height, fused_map)); + + // copy 2nd temp to temp buffer + gridmap_dict_ = gridmap_dict_tmp_; + std::for_each(std::begin(gridmap_dict_tmp_), std::end(gridmap_dict_tmp_), [](auto & e) { + e.second = nullptr; + }); + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( + std::vector & occupancy_grid_maps, + const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights) +{ + // if only single map + if (occupancy_grid_maps.size() == 1) { + return occupancy_grid_maps[0]; + } + + // get map to gridmap_origin_frame_ transform + Pose gridmap_origin{}; + try { + gridmap_origin = utils::getPose(latest_stamp, tf_buffer_, gridmap_origin_frame_, map_frame_); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(get_logger(), ex.what()); + return occupancy_grid_maps[0]; + } + + // init fused map with calculated origin + OccupancyGridMapFixedBlindSpot fused_map( + occupancy_grid_maps[0].getSizeInCellsX(), occupancy_grid_maps[0].getSizeInCellsY(), + occupancy_grid_maps[0].getResolution()); + fused_map.updateOrigin( + gridmap_origin.position.x - fused_map.getSizeInMetersX() / 2, + gridmap_origin.position.y - fused_map.getSizeInMetersY() / 2); + + // fix origin of each map + for (auto & map : occupancy_grid_maps) { + map.updateOrigin(fused_map.getOriginX(), fused_map.getOriginY()); + } + + // assume map is same size and resolutions + for (unsigned int x = 0; x < fused_map.getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < fused_map.getSizeInCellsY(); y++) { + // get cost of each map + std::vector costs; + for (auto & map : occupancy_grid_maps) { + costs.push_back(map.getCost(x, y)); + } + + // set fusion policy + auto fused_cost = fusion_policy::singleFrameOccupancyFusion(costs, fusion_method_, weights); + + // set max cost to fused map + fused_map.setCost(x, y, fused_cost); + } + } + + return fused_map; +} + +/** + * @brief Update occupancy grid map with asynchronous input (currently unused) + * + * @param occupancy_grid_msg + */ +void GridMapFusionNode::updateGridMap( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & occupancy_grid_msg) +{ + // get updater map origin + + // origin is set to current updater map + auto map_for_update = OccupancyGridMsgToCostmap2D(*occupancy_grid_msg); + + // update + occupancy_grid_map_updater_ptr_->update(map_for_update); +} + +nav2_costmap_2d::Costmap2D GridMapFusionNode::OccupancyGridMsgToCostmap2D( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + nav2_costmap_2d::Costmap2D costmap2d( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution, occupancy_grid_map.info.origin.position.x, + occupancy_grid_map.info.origin.position.y, 0); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + costmap2d.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + + return costmap2d; +} + +OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( + const nav_msgs::msg::OccupancyGrid & occupancy_grid_map) +{ + OccupancyGridMapFixedBlindSpot gridmap( + occupancy_grid_map.info.width, occupancy_grid_map.info.height, + occupancy_grid_map.info.resolution); + gridmap.updateOrigin( + occupancy_grid_map.info.origin.position.x, occupancy_grid_map.info.origin.position.y); + + for (unsigned int i = 0; i < occupancy_grid_map.info.width; i++) { + for (unsigned int j = 0; j < occupancy_grid_map.info.height; j++) { + const unsigned int index = i + j * occupancy_grid_map.info.width; + gridmap.setCost( + i, j, occupancy_cost_value::inverse_cost_translation_table[occupancy_grid_map.data[index]]); + } + } + return gridmap; +} + +nav_msgs::msg::OccupancyGrid::UniquePtr GridMapFusionNode::OccupancyGridMapToMsgPtr( + const std::string & frame_id, const builtin_interfaces::msg::Time & stamp, + const float & robot_pose_z, const nav2_costmap_2d::Costmap2D & occupancy_grid_map) +{ + auto msg_ptr = std::make_unique(); + + msg_ptr->header.frame_id = frame_id; + msg_ptr->header.stamp = stamp; + msg_ptr->info.resolution = occupancy_grid_map.getResolution(); + + msg_ptr->info.width = occupancy_grid_map.getSizeInCellsX(); + msg_ptr->info.height = occupancy_grid_map.getSizeInCellsY(); + + double wx{}; + double wy{}; + occupancy_grid_map.mapToWorld(0, 0, wx, wy); + msg_ptr->info.origin.position.x = occupancy_grid_map.getOriginX(); + msg_ptr->info.origin.position.y = occupancy_grid_map.getOriginY(); + msg_ptr->info.origin.position.z = robot_pose_z; + msg_ptr->info.origin.orientation.w = 1.0; + + msg_ptr->data.resize(msg_ptr->info.width * msg_ptr->info.height); + + unsigned char * data = occupancy_grid_map.getCharMap(); + for (unsigned int i = 0; i < msg_ptr->data.size(); ++i) { + msg_ptr->data[i] = occupancy_cost_value::cost_translation_table[data[i]]; + } + return msg_ptr; +} + +} // namespace synchronized_grid_map_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(synchronized_grid_map_fusion::GridMapFusionNode) 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 new file mode 100644 index 0000000000000..f3e306f262bf0 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp @@ -0,0 +1,69 @@ +// 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 "probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp" + +#include "probabilistic_occupancy_grid_map/cost_value.hpp" + +#include + +namespace costmap_2d +{ + +void OccupancyGridMapLOBFUpdater::initRosParam(rclcpp::Node & /*node*/) +{ + // nothing to load +} + +inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( + const unsigned char & z, const unsigned char & o) +{ + using fusion_policy::convertCharToProbability; + using fusion_policy::convertProbabilityToChar; + using fusion_policy::log_odds_fusion::logOddsFusion; + + constexpr unsigned char unknown = occupancy_cost_value::NO_INFORMATION; + constexpr unsigned char unknown_margin = 1; + /* Tau and ST decides how fast the observation decay to the unknown status*/ + constexpr double tau = 0.75; + constexpr double sample_time = 0.1; + + // if the observation is unknown, decay the estimation + if (z >= unknown - unknown_margin && z <= unknown + unknown_margin) { + char diff = static_cast(o) - static_cast(unknown); + const double decay = std::exp(-sample_time / tau); + const double fused = static_cast(unknown) + static_cast(diff) * decay; + return static_cast(fused); + } else { + // else, do the log-odds fusion + const std::vector probability = { + convertCharToProbability(z), convertCharToProbability(o)}; + const unsigned char fused = convertProbabilityToChar(logOddsFusion(probability)); + return fused; + } +} + +bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map) +{ + updateOrigin( + single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY()); + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyLOBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + } + } + return true; +} +} // namespace costmap_2d diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 0060754cd875c..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 @@ -182,4 +182,20 @@ bool extractCommonPointCloud( return true; } +/** + * @brief Convert unsigned char value to closest cost value + * @param cost Cost value + * @return Probability + */ +unsigned char getApproximateOccupancyState(const unsigned char & value) +{ + if (value >= occupancy_cost_value::OCCUPIED_THRESHOLD) { + return occupancy_cost_value::LETHAL_OBSTACLE; + } else if (value <= occupancy_cost_value::FREE_THRESHOLD) { + return occupancy_cost_value::FREE_SPACE; + } else { + return occupancy_cost_value::NO_INFORMATION; + } +} + } // namespace utils diff --git a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md new file mode 100644 index 0000000000000..6f4d4e22aa26e --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -0,0 +1,168 @@ +# synchronized OGM fusion + +> For simplicity, we use OGM as the meaning of the occupancy grid map. + +This package is used to fuse the OGMs from synchronized sensors. Especially for the lidar. + +Here shows the example OGM for the this synchronized OGM fusion. + +| left lidar OGM | right lidar OGM | top lidar OGM | +| --------------------------------- | ----------------------------------- | ------------------------------- | +| ![left](image/left_lidar_ogm.png) | ![right](image/right_lidar_ogm.png) | ![top](image/top_lidar_ogm.png) | + +OGM fusion with asynchronous sensor outputs is not suitable for this package. Asynchronous OGM fusion is under construction. + +## Processing flow + +The processing flow of this package is shown in the following figure. + +![data_flow](image/synchronized_grid_map_fusion.drawio.svg) + +- Single Frame Fusion + - Single frame fusion means that the OGMs from synchronized sensors are fused in a certain time frame $t=t_n$. +- Multi Frame Fusion + - In the multi frame fusion process, current fused single frame OGM in $t_n$ is fused with the previous fused single frame OGM in $t_{n-1}$. + +## I/O + +| Input topic name | Type | Description | +| ------------------ | ------------------------------------ | --------------------------------------------------------------------------------- | +| `input_ogm_topics` | list of nav_msgs::msg::OccupancyGrid | List of input topics for Occupancy Grid Maps. This parameter is given in list, so | + +| Output topic name | Type | Description | +| ----------------------------- | ---------------------------- | ----------------------------------------------------------------------------- | +| `~/output/occupancy_grid_map` | nav_msgs::msg::OccupancyGrid | Output topic name of the fused Occupancy Grid Map. | +| `~/debug/single_frame_map` | nav_msgs::msg::OccupancyGrid | (debug topic) Output topic name of the single frame fused Occupancy Grid Map. | + +## Parameters + +Synchronized OGM fusion node parameters are shown in the following table. Main parameters to be considered in the fusion node is shown as bold. + +| Ros param name | Sample value | Description | +| --------------------------- | -------------------- | ------------------------------------------------------------- | +| **input_ogm_topics** | ["topic1", "topic2"] | List of input topics for Occupancy Grid Maps | +| **input_ogm_reliabilities** | [0.8, 0.2] | Weights for the reliability of each input topic | +| **fusion_method** | "overwrite" | Method of fusion ("overwrite", "log-odds", "dempster-shafer") | +| match_threshold_sec | 0.01 | Matching threshold in milliseconds | +| timeout_sec | 0.1 | Timeout duration in seconds | +| input_offset_sec | [0.0, 0.0] | Offset time in seconds for each input topic | +| map*frame* | "map" | Frame name for the fused map | +| base*link_frame* | "base_link" | Frame name for the base link | +| grid*map_origin_frame* | "base_link" | Frame name for the origin of the grid map | +| fusion_map_length_x | 100.0 | Length of the fused map along the X-axis | +| fusion_map_length_y | 100.0 | Length of the fused map along the Y-axis | +| fusion_map_resolution | 0.5 | Resolution of the fused map | + +Since this node assumes that the OGMs from synchronized sensors are generated in the same time, we need to tune the `match_threshold_sec`, `timeout_sec` and `input_offset_sec` parameters to successfully fuse the OGMs. + +## Fusion methods + +For the single frame fusion, the following fusion methods are supported. + +| Fusion Method in parameter | Description | +| -------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `overwrite` | The value of the cell in the fused OGM is overwritten by the value of the cell in the OGM with the highest priority.
We set priority as `Occupied` > `Free` > `Unknown`. | +| `log-odds` | The value of the cell in the fused OGM is calculated by the log-odds ratio method, which is known as a Bayesian fusion method.
The log-odds of a probability $p$ can be written as $l_p = \log(\frac{p}{1-p})$.
And the fused log-odds is calculated by the sum of log-odds. $l_f = \Sigma l_p$ | +| `dempster-shafer` | The value of the cell in the fused OGM is calculated by the Dempster-Shafer theory[1]. This is also popular method to handle multiple evidences. This package applied conflict escape logic in [2] for the performance. See references for the algorithm details. | + +For the multi frame fusion, currently only supporting `log-odds` fusion method. + +## How to use + +### launch fusion node + +The minimum node launch will be like the following. + +```xml + + + + + + + + + + +``` + +### (Optional) Generate OGMs in each sensor frame + +You need to generate OGMs in each sensor frame before achieving grid map fusion. + +`probabilistic_occupancy_grid_map` package supports to generate OGMs for the each from the point cloud data. + +
+ Example launch.xml (click to expand) + +```xml + + + + + + + + + + + + + + + +The minimum parameter for the OGM generation in each frame is shown in the following table. + +|Parameter|Description| +|--|--| +|`input/obstacle_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is segmented as the obstacle.| +|`input/raw_pointcloud`| The input point cloud data for the OGM generation. This point cloud data should be the point cloud data which is not segmented as the obstacle. | +|`output`| The output topic of the OGM. | +|`map_frame`| The tf frame for the OGM center origin. | +|`scan_origin`| The tf frame for the sensor origin. | +|`method`| The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +|`param_file`| The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +``` + +
+ +
+ +We recommend to use same `map_frame`, size and resolutions for the OGMs from synchronized sensors. +Also, remember to set `enable_single_frame_mode` and `filter_obstacle_pointcloud_by_raw_pointcloud` to `true` in the `probabilistic_occupancy_grid_map` package (you do not need to set these parameters if you use the above example config file). + +
+ +### Run both OGM generation node and fusion node + +We prepared the launch file to run both OGM generation node and fusion node in [`grid_map_fusion_with_synchronized_pointclouds.launch.py`](launch/grid_map_fusion_with_synchronized_pointclouds.launch.py) + +You can include this launch file like the following. + +```xml + + + + + + + + + + +``` + +The minimum parameter for the launch file is shown in the following table. + +| Parameter | Description | +| -------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `output` | The output topic of the finally fused OGM. | +| `method` | The method for the OGM generation. Currently we support `pointcloud_based_occupancy_grid_map` and `laser_scan_based_occupancy_grid_map`. The pointcloud based method is recommended. | +| `fusion_config_file` | The parameter file for the grid map fusion. See [example parameter file](config/grid_map_fusion.param.yaml) | +| `ogm_config_file` | The parameter file for the OGM generation. See [example parameter file](config/pointcloud_based_occupancy_grid_map_for_fusion.param.yaml) | + +## References + +- [1] Dempster, A. P., Laird, N. M., & Rubin, D. B. (1977). Maximum likelihood from incomplete data via the EM algorithm. Journal of the Royal Statistical Society. Series B (Methodological), 39(1), 1-38. +- [2] diff --git a/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp new file mode 100644 index 0000000000000..c41c809a92b0c --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/cost_value_test.cpp @@ -0,0 +1,47 @@ +// 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 "probabilistic_occupancy_grid_map/cost_value.hpp" + +#include + +// Test the CostTranslationTable and InverseCostTranslationTable functions +using occupancy_cost_value::cost_translation_table; +using occupancy_cost_value::inverse_cost_translation_table; + +TEST(CostTranslationTableTest, TestRange) +{ + // Test the value range of CostTranslationTable + for (int i = 0; i < 256; i++) { + EXPECT_GE(cost_translation_table[i], 1); + EXPECT_LE(cost_translation_table[i], 99); + } + + // Test the value range of InverseCostTranslationTable + for (int i = 1; i < 100; i++) { + EXPECT_GE(inverse_cost_translation_table[i], 0); + EXPECT_LE(inverse_cost_translation_table[i], 255); + } +} + +TEST(CostTranslationTableTest, TestConversion) +{ + // Test the forward and inverse conversion of 0, 128, and 255 + EXPECT_EQ(cost_translation_table[0], 1); + EXPECT_EQ(cost_translation_table[128], 50); + EXPECT_EQ(cost_translation_table[255], 99); + EXPECT_EQ(inverse_cost_translation_table[1], 2); + EXPECT_EQ(inverse_cost_translation_table[50], 128); + EXPECT_EQ(inverse_cost_translation_table[99], 255); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp new file mode 100644 index 0000000000000..6b3dc8a2ebcef --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp @@ -0,0 +1,76 @@ +// 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 "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) +{ + using fusion_policy::log_odds_fusion::logOddsFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(logOddsFusion(case1_1), OCCUPIED); + EXPECT_LE(logOddsFusion(case1_2), FREE); + EXPECT_NEAR(logOddsFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(logOddsFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(logOddsFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(logOddsFusion(case3_1), UNKNOWN, EPSILON); +} + +// Test the dempster-shafer update rule +TEST(FusionPolicyTest, TestDempsterShaferUpdateRule) +{ + using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion; + const double MARGIN = 0.03; + const double OCCUPIED = 1.0 - MARGIN; + const double FREE = 0.0 + MARGIN; + const double UNKNOWN = 0.5; + const double EPSILON = 1e-3; + + // same case + std::vector case1_1 = {OCCUPIED, OCCUPIED}; + std::vector case1_2 = {FREE, FREE}; + std::vector case1_3 = {UNKNOWN, UNKNOWN}; + EXPECT_GE(dempsterShaferFusion(case1_1), OCCUPIED); + EXPECT_LE(dempsterShaferFusion(case1_2), FREE); + EXPECT_NEAR(dempsterShaferFusion(case1_3), UNKNOWN, EPSILON); + + // with unknown + std::vector case2_1 = {OCCUPIED, UNKNOWN}; + std::vector case2_2 = {FREE, UNKNOWN}; + EXPECT_NEAR(dempsterShaferFusion(case2_1), OCCUPIED, EPSILON); + EXPECT_NEAR(dempsterShaferFusion(case2_2), FREE, EPSILON); + + // with conflict + std::vector case3_1 = {OCCUPIED, FREE}; + EXPECT_NEAR(dempsterShaferFusion(case3_1), UNKNOWN, EPSILON); +} diff --git a/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py new file mode 100644 index 0000000000000..c15daaf886b6f --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py @@ -0,0 +1,255 @@ +# 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. + +import time +import unittest + +from geometry_msgs.msg import TransformStamped +import launch +import launch.actions +from launch_ros.substitutions import FindPackageShare +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.markers +import launch_testing.tools +from nav_msgs.msg import OccupancyGrid +import numpy as np +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +PACKAGE_NAME = "probabilistic_occupancy_grid_map" +INPUT_TOPICS = ["/topic1", "/topic2"] +DEBUG_OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/debug/single_frame_map" +OUTPUT_TOPIC = "/synchronized_grid_map_fusion_node/output/occupancy_grid_map" + +FREE_VALUE = 1 +UNKNOWN_VALUE = 50 +OCCUPIED_VALUE = 99 + + +# test launcher to launch grid map fusion node +@pytest.mark.launch_test +def generate_test_description(): + """Launch file test description. + + Returns: + _type_: launch.LaunchDescription + """ + # get launch file path + launch_file_path = ( + FindPackageShare(PACKAGE_NAME).find(PACKAGE_NAME) + + "/launch/synchronized_occupancy_grid_map_fusion.launch.xml" + ) + # use default launch arguments and parms + launch_args = [] + # action to include launch file + test_launch_file = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.AnyLaunchDescriptionSource(launch_file_path), + launch_arguments=launch_args, + ) + + return launch.LaunchDescription( + [ + test_launch_file, + launch_testing.actions.ReadyToTest(), + ] + ) + + +# util functions +def create_nav_msgs_occupancy_grid_msg(fill_value: int = 0, stamp: rclpy.time.Time = None): + """Create nav_msgs occupancy grid message. + + Args: + fill_value (int, optional): fill value. Defaults to 0. + + Returns: + OccupancyGrid: nav_msgs occupancy grid message + """ + msg = OccupancyGrid() + msg.header.stamp = rclpy.clock.Clock().now().to_msg() if stamp is None else stamp.to_msg() + msg.header.frame_id = "map" + msg.info.resolution = 0.5 # 0.5m x 0.5m + msg.info.width = 200 # 100m x 100m + msg.info.height = 200 + msg.info.origin.position.x = -msg.info.width * msg.info.resolution / 2.0 + msg.info.origin.position.y = -msg.info.height * msg.info.resolution / 2.0 + msg.info.origin.position.z = 0.0 + msg.info.origin.orientation.x = 0.0 + msg.info.origin.orientation.y = 0.0 + msg.info.origin.orientation.z = 0.0 + msg.info.origin.orientation.w = 1.0 + msg.data = [fill_value] * msg.info.width * msg.info.height + return msg + + +def parse_ogm_msg(msg: OccupancyGrid): + """Parse nav_msgs occupancy grid message. + + Args: + msg (OccupancyGrid): nav_msgs occupancy grid message + + Returns: + np.ndarray: occupancy grid map + """ + ogm = np.array(msg.data).reshape((msg.info.height, msg.info.width)) + return ogm + + +# dummy tf broadcaster +def generate_static_transform_msg(stamp: rclpy.time.Time = None): + """Generate static transform message from base_link to map. + + Returns: + TransformStamped: static transform message + """ + msg = TransformStamped() + if stamp is None: + msg.header.stamp = rclpy.clock.Clock().now().to_msg() + else: + msg.header.stamp = stamp.to_msg() + msg.header.frame_id = "map" + msg.child_frame_id = "base_link" + msg.transform.translation.x = 0.0 + msg.transform.translation.y = 0.0 + msg.transform.translation.z = 0.0 + msg.transform.rotation.x = 0.0 + msg.transform.rotation.y = 0.0 + msg.transform.rotation.z = 0.0 + msg.transform.rotation.w = 1.0 + return msg + + +# --- TestSynchronizedOGMFusion --- +# 1. test free ogm and free ogm input fusion +# 2. test occupied ogm and occupied ogm input fusion +# 3. test unknown ogm and free ogm input fusion +# 4. test unknown ogm and occupied ogm input fusion +# 5. test free ogm and occupied ogm input fusion +class TestSynchronizedOGMFusion(unittest.TestCase): + @classmethod + def setUpClass(cls): + # init ROS at once + rclpy.init() + + @classmethod + def tearDownClass(cls): + # shutdown ROS at once + rclpy.shutdown() + + def setUp(self): + # called when each test started + self.node = rclpy.create_node("grid_map_fusion_node_test_node") + # send static transform from map to base_link + tf_msg = generate_static_transform_msg() + self.tf_broadcaster = StaticTransformBroadcaster(self.node) + self.tf_broadcaster.sendTransform(tf_msg) + + def tearDown(self): + # called when each test finished + self.node.destroy_node() + + def callback(self, msg: OccupancyGrid): + self.msg_buffer.append(msg) + print("callback", len(self.msg_buffer)) + + def get_newest_ogm_value(self): + return np.mean(self.msg_buffer[-1].data) + + # util functions test 1~3 + def create_pub_sub(self): + # create publisher + pub1 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[0], 10) + pub2 = self.node.create_publisher(OccupancyGrid, INPUT_TOPICS[1], 10) + + # create subscriber for debug output + sensor_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + ) + self.msg_buffer = [] + # subscribe to occupancy grid with buffer + sub = self.node.create_subscription( + OccupancyGrid, DEBUG_OUTPUT_TOPIC, self.callback, qos_profile=sensor_qos + ) + return [pub1, pub2], sub + + # test functions + def test_same_ogm_fusion(self): + """Test 1~3. + + Expected output: same ogm. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() # pubs have two publishers + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + + for fill_value in fill_values: + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value) + # publish free/occupied/unknown ogm + pubs[0].publish(ogm) + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("same ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + def test_unknown_fusion(self): + """Test unknown ogm and free ogm input fusion. + + Expected output: free, occupied, unknown. + """ + # wait for the node to be ready + time.sleep(3) + pubs, sub = self.create_pub_sub() + + fill_values = [FREE_VALUE, OCCUPIED_VALUE, UNKNOWN_VALUE] + now = rclpy.clock.Clock().now() + unknown_ogm = create_nav_msgs_occupancy_grid_msg(fill_value=UNKNOWN_VALUE, stamp=now) + + for fill_value in fill_values: + # publish unknown ogm + pubs[0].publish(unknown_ogm) + # create free/occupied/unknown ogm + ogm = create_nav_msgs_occupancy_grid_msg(fill_value=fill_value, stamp=now) + # publish ogm + pubs[1].publish(ogm) + + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=2.0) + fused_mean = self.get_newest_ogm_value() + print("unknown ogm test: ", fill_value, fused_mean) + # assert almost equal + self.assertAlmostEqual(fused_mean, fill_value, delta=3.0) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # check exit code + launch_testing.asserts.assertExitCodes(proc_info) 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_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp similarity index 94% rename from perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp rename to perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp index b5f2005a84baf..0d2f6fd10a6b6 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ -#define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ #include "rclcpp/logger.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" @@ -115,4 +115,4 @@ class RadarFusionToDetectedObject }; } // namespace radar_fusion_to_detected_object -#endif // RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#endif // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ diff --git a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp similarity index 89% rename from perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp rename to perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp index 1a90e9f6bd046..837ef45c65c60 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp @@ -13,13 +13,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ -#define RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ #include "message_filters/subscriber.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" -#include "radar_fusion_to_detected_object.hpp" +#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" @@ -89,4 +89,4 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node } // namespace radar_fusion_to_detected_object -#endif // RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT__RADAR_OBJECT_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#endif // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ diff --git a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json b/perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json similarity index 97% rename from perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json rename to perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json index 688414df56c1e..69c83503a2220 100644 --- a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json +++ b/perception/radar_fusion_to_detected_object/schema/radar_object_fusion_to_detected_object.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Radar Fusion to Detected Object Node", "type": "object", "definitions": { - "radar_fusion_to_detected_object": { + "radar_object_fusion_to_detected_object": { "type": "object", "properties": { "node_params": { @@ -104,7 +104,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/radar_fusion_to_detected_object" + "$ref": "#/definitions/radar_object_fusion_to_detected_object" } }, "required": ["ros__parameters"] diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 75684d51039fd..43057e685736c 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -13,7 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_fusion_to_detected_object.hpp" +#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" #include #include diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index b233bdf31adc6..23e6d43422fa1 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -13,8 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp" - +#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 54ef7b047bf17..7c1b98c10d14b 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(nlohmann_json REQUIRED) # for debug +find_package(glog REQUIRED) include_directories( SYSTEM @@ -32,6 +33,7 @@ target_link_libraries(radar_object_tracker_node Eigen3::Eigen yaml-cpp nlohmann_json::nlohmann_json # for debug + glog::glog ) rclcpp_components_register_node(radar_object_tracker_node diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index ad71e613c1a18..6939c54b5be75 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -17,6 +17,7 @@ eigen kalman_filter lanelet2_extension + libgoogle-glog-dev mussp nlohmann-json-dev object_recognition_utils diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 6d801302ab6c5..e4b394256101d 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -23,6 +23,7 @@ #include +#include #include #include @@ -194,6 +195,10 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("radar_object_tracker"); + google::InstallFailureSignalHandler(); + // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index c29d8ee424549..fa08eb08f521a 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -1,6 +1,6 @@ # radar_tracks_msgs_converter -This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). +This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) and [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl). - Calculation cost is O(n). - n: The number of radar objects @@ -20,10 +20,14 @@ This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-pe - `update_rate_hz` (double): The update rate [hz]. - Default parameter is 20.0 -- `new_frame_id` (string): The header frame of output topic. +- `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 output objects' topic is compensated by ego vehicle motion. +- `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion. + - Default parameter is "true" +- `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. - Default parameter is "false" +- `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s]. + - Default parameter is 1.0 ## Note @@ -42,4 +46,5 @@ Label id is defined as below. | PEDESTRIAN | 32007 | 7 | - [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg): additional vendor-specific classifications are permitted starting from 32000. + - [Autoware objects label](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl) diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 351410161d8b2..53a2a8655b9dd 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-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. @@ -43,6 +43,7 @@ using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; using nav_msgs::msg::Odometry; +using radar_msgs::msg::RadarTrack; using radar_msgs::msg::RadarTracks; class RadarTracksMsgsConverterNode : public rclcpp::Node @@ -55,6 +56,8 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node double update_rate_hz{}; std::string new_frame_id{}; bool use_twist_compensation{}; + bool use_twist_yaw_compensation{}; + double static_object_speed_threshold{}; }; private: @@ -94,6 +97,20 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node geometry_msgs::msg::PoseWithCovariance convertPoseWithCovariance(); TrackedObjects convertRadarTrackToTrackedObjects(); DetectedObjects convertTrackedObjectsToDetectedObjects(TrackedObjects & objects); + geometry_msgs::msg::Vector3 compensateVelocitySensorPosition( + const radar_msgs::msg::RadarTrack & radar_track); + geometry_msgs::msg::Vector3 compensateVelocityEgoMotion( + const geometry_msgs::msg::Vector3 & velocity_in, + const geometry_msgs::msg::Point & position_from_veh); + bool isStaticObject( + const radar_msgs::msg::RadarTrack & radar_track, + const geometry_msgs::msg::Vector3 & compensated_velocity); + std::array convertPoseCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track); + std::array convertTwistCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track); + std::array convertAccelerationCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track); uint8_t convertClassification(const uint16_t classification); }; 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 c9f4a31354247..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,9 @@ - + + + @@ -13,5 +15,7 @@ + + diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index d8b5599cc1ca7..323e4bf7788c5 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-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. @@ -81,6 +81,10 @@ RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOpt node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); node_param_.use_twist_compensation = declare_parameter("use_twist_compensation", false); + node_param_.use_twist_yaw_compensation = + declare_parameter("use_twist_yaw_compensation", false); + node_param_.static_object_speed_threshold = + declare_parameter("static_object_speed_threshold", 1.0); // Subscriber sub_radar_ = create_subscription( @@ -125,6 +129,8 @@ rcl_interfaces::msg::SetParametersResult RadarTracksMsgsConverterNode::onSetPara update_param(params, "update_rate_hz", p.update_rate_hz); update_param(params, "new_frame_id", p.new_frame_id); update_param(params, "use_twist_compensation", p.use_twist_compensation); + update_param(params, "use_twist_yaw_compensation", p.use_twist_yaw_compensation); + update_param(params, "static_object_speed_threshold", p.static_object_speed_threshold); } } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; @@ -192,13 +198,32 @@ DetectedObjects RadarTracksMsgsConverterNode::convertTrackedObjectsToDetectedObj return detected_objects; } +bool RadarTracksMsgsConverterNode::isStaticObject( + const radar_msgs::msg::RadarTrack & radar_track, + const geometry_msgs::msg::Vector3 & compensated_velocity) +{ + if (!(node_param_.use_twist_compensation && odometry_data_)) { + return false; + } + + // Calculate azimuth angle of the object in the vehicle coordinate + const double sensor_yaw = tf2::getYaw(transform_->transform.rotation); + const double radar_azimuth = std::atan2(radar_track.position.y, radar_track.position.x); + const double azimuth = radar_azimuth + sensor_yaw; + + // Calculate longitudinal speed + const double longitudinal_speed = + compensated_velocity.x * std::cos(azimuth) + compensated_velocity.y * std::sin(azimuth); + + // Check if the object is static + return std::abs(longitudinal_speed) < node_param_.static_object_speed_threshold; +} + TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() { TrackedObjects tracked_objects; tracked_objects.header = radar_data_->header; tracked_objects.header.frame_id = node_param_.new_frame_id; - using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; for (auto & radar_track : radar_data_->tracks) { TrackedObject tracked_object; @@ -209,40 +234,10 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() tracked_object.shape.type = Shape::BOUNDING_BOX; tracked_object.shape.dimensions = radar_track.size; - // kinematics setting - TrackedObjectKinematics kinematics; - kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE; - kinematics.is_stationary = false; - - geometry_msgs::msg::Vector3 compensated_velocity{}; - { - double rotate_yaw = tf2::getYaw(transform_->transform.rotation); - const geometry_msgs::msg::Vector3 & vel = radar_track.velocity; - compensated_velocity.x = vel.x * std::cos(rotate_yaw) - vel.y * std::sin(rotate_yaw); - compensated_velocity.y = vel.x * std::sin(rotate_yaw) + vel.y * std::cos(rotate_yaw); - compensated_velocity.z = radar_track.velocity.z; - } - - if (node_param_.use_twist_compensation) { - if (odometry_data_) { - compensated_velocity.x += odometry_data_->twist.twist.linear.x; - compensated_velocity.y += odometry_data_->twist.twist.linear.y; - compensated_velocity.z += odometry_data_->twist.twist.linear.z; - } else { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Odometry data is not coming"); - } - } - kinematics.twist_with_covariance.twist.linear.x = std::sqrt( - compensated_velocity.x * compensated_velocity.x + - compensated_velocity.y * compensated_velocity.y); - // Pose conversion geometry_msgs::msg::PoseStamped radar_pose_stamped{}; radar_pose_stamped.pose.position = radar_track.position; - double yaw = tier4_autoware_utils::normalizeRadian( - std::atan2(compensated_velocity.y, compensated_velocity.x)); - geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; if (transform_ == nullptr) { RCLCPP_ERROR_THROTTLE( @@ -250,50 +245,47 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() return tracked_objects; } tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); + const auto & position_from_veh = transformed_pose_stamped.pose.position; + + // Velocity conversion + // 1: Compensate radar coordinate + // radar track velocity is defined in the radar coordinate + // compensate radar coordinate to vehicle coordinate + auto compensated_velocity = compensateVelocitySensorPosition(radar_track); + // 2: Compensate ego motion + if (node_param_.use_twist_compensation && odometry_data_) { + compensated_velocity = compensateVelocityEgoMotion(compensated_velocity, position_from_veh); + } else if (node_param_.use_twist_compensation && !odometry_data_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Odometry data is not available. Radar track velocity will not be compensated."); + } + + // yaw angle (vehicle heading) is obtained from ground velocity + double yaw = tier4_autoware_utils::normalizeRadian( + std::atan2(compensated_velocity.y, compensated_velocity.x)); + + // kinematics setting + TrackedObjectKinematics kinematics; + kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE; + kinematics.is_stationary = isStaticObject(radar_track, compensated_velocity); kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; + kinematics.pose_with_covariance.covariance = convertPoseCovarianceMatrix(radar_track); + // velocity of object is defined in the object coordinate + // heading is obtained from ground velocity kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - { - auto & pose_cov = kinematics.pose_with_covariance.covariance; - auto & radar_position_cov = radar_track.position_covariance; - pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X]; - pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y]; - pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z]; - pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y]; - pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y]; - pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z]; - pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z]; - pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z]; - pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z]; - } - - { - auto & twist_cov = kinematics.twist_with_covariance.covariance; - auto & radar_vel_cov = radar_track.velocity_covariance; - twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X]; - twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y]; - twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z]; - twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y]; - twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y]; - twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z]; - twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z]; - twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z]; - twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z]; - } - { - auto & accel_cov = kinematics.acceleration_with_covariance.covariance; - auto & radar_accel_cov = radar_track.acceleration_covariance; - accel_cov[POSE_IDX::X_X] = radar_accel_cov[RADAR_IDX::X_X]; - accel_cov[POSE_IDX::X_Y] = radar_accel_cov[RADAR_IDX::X_Y]; - accel_cov[POSE_IDX::X_Z] = radar_accel_cov[RADAR_IDX::X_Z]; - accel_cov[POSE_IDX::Y_X] = radar_accel_cov[RADAR_IDX::X_Y]; - accel_cov[POSE_IDX::Y_Y] = radar_accel_cov[RADAR_IDX::Y_Y]; - accel_cov[POSE_IDX::Y_Z] = radar_accel_cov[RADAR_IDX::Y_Z]; - accel_cov[POSE_IDX::Z_X] = radar_accel_cov[RADAR_IDX::X_Z]; - accel_cov[POSE_IDX::Z_Y] = radar_accel_cov[RADAR_IDX::Y_Z]; - accel_cov[POSE_IDX::Z_Z] = radar_accel_cov[RADAR_IDX::Z_Z]; - } + // longitudinal velocity is the length of the velocity vector + // lateral velocity is zero, use default value + kinematics.twist_with_covariance.twist.linear.x = std::sqrt( + compensated_velocity.x * compensated_velocity.x + + compensated_velocity.y * compensated_velocity.y); + kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(radar_track); + // acceleration is zero, use default value + kinematics.acceleration_with_covariance.covariance = + convertAccelerationCovarianceMatrix(radar_track); + // fill the kinematics to the tracked object tracked_object.kinematics = kinematics; // classification @@ -307,6 +299,91 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() return tracked_objects; } +geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocitySensorPosition( + const radar_msgs::msg::RadarTrack & radar_track) +{ + // initialize compensated velocity + geometry_msgs::msg::Vector3 compensated_velocity{}; + + const double sensor_yaw = tf2::getYaw(transform_->transform.rotation); + const geometry_msgs::msg::Vector3 & vel = radar_track.velocity; + compensated_velocity.x = vel.x * std::cos(sensor_yaw) - vel.y * std::sin(sensor_yaw); + compensated_velocity.y = vel.x * std::sin(sensor_yaw) + vel.y * std::cos(sensor_yaw); + compensated_velocity.z = vel.z; + + return compensated_velocity; +} + +geometry_msgs::msg::Vector3 RadarTracksMsgsConverterNode::compensateVelocityEgoMotion( + const geometry_msgs::msg::Vector3 & velocity_in, + const geometry_msgs::msg::Point & position_from_veh) +{ + geometry_msgs::msg::Vector3 velocity = velocity_in; + // linear compensation + velocity.x += odometry_data_->twist.twist.linear.x; + velocity.y += odometry_data_->twist.twist.linear.y; + velocity.z += odometry_data_->twist.twist.linear.z; + if (node_param_.use_twist_yaw_compensation) { + // angular compensation + const double veh_yaw = odometry_data_->twist.twist.angular.z; + velocity.x += -position_from_veh.y * veh_yaw; + velocity.y += position_from_veh.x * veh_yaw; + } + return velocity; +} + +std::array RadarTracksMsgsConverterNode::convertPoseCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + std::array pose_covariance{}; + pose_covariance[POSE_IDX::X_X] = radar_track.position_covariance[RADAR_IDX::X_X]; + pose_covariance[POSE_IDX::X_Y] = radar_track.position_covariance[RADAR_IDX::X_Y]; + pose_covariance[POSE_IDX::X_Z] = radar_track.position_covariance[RADAR_IDX::X_Z]; + pose_covariance[POSE_IDX::Y_X] = radar_track.position_covariance[RADAR_IDX::X_Y]; + pose_covariance[POSE_IDX::Y_Y] = radar_track.position_covariance[RADAR_IDX::Y_Y]; + pose_covariance[POSE_IDX::Y_Z] = radar_track.position_covariance[RADAR_IDX::Y_Z]; + pose_covariance[POSE_IDX::Z_X] = radar_track.position_covariance[RADAR_IDX::X_Z]; + pose_covariance[POSE_IDX::Z_Y] = radar_track.position_covariance[RADAR_IDX::Y_Z]; + pose_covariance[POSE_IDX::Z_Z] = radar_track.position_covariance[RADAR_IDX::Z_Z]; + return pose_covariance; +} +std::array RadarTracksMsgsConverterNode::convertTwistCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + std::array twist_covariance{}; + twist_covariance[POSE_IDX::X_X] = radar_track.velocity_covariance[RADAR_IDX::X_X]; + twist_covariance[POSE_IDX::X_Y] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; + twist_covariance[POSE_IDX::X_Z] = radar_track.velocity_covariance[RADAR_IDX::X_Z]; + twist_covariance[POSE_IDX::Y_X] = radar_track.velocity_covariance[RADAR_IDX::X_Y]; + twist_covariance[POSE_IDX::Y_Y] = radar_track.velocity_covariance[RADAR_IDX::Y_Y]; + twist_covariance[POSE_IDX::Y_Z] = radar_track.velocity_covariance[RADAR_IDX::Y_Z]; + twist_covariance[POSE_IDX::Z_X] = radar_track.velocity_covariance[RADAR_IDX::X_Z]; + twist_covariance[POSE_IDX::Z_Y] = radar_track.velocity_covariance[RADAR_IDX::Y_Z]; + twist_covariance[POSE_IDX::Z_Z] = radar_track.velocity_covariance[RADAR_IDX::Z_Z]; + return twist_covariance; +} +std::array RadarTracksMsgsConverterNode::convertAccelerationCovarianceMatrix( + const radar_msgs::msg::RadarTrack & radar_track) +{ + using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX; + std::array acceleration_covariance{}; + acceleration_covariance[POSE_IDX::X_X] = radar_track.acceleration_covariance[RADAR_IDX::X_X]; + acceleration_covariance[POSE_IDX::X_Y] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; + acceleration_covariance[POSE_IDX::X_Z] = radar_track.acceleration_covariance[RADAR_IDX::X_Z]; + acceleration_covariance[POSE_IDX::Y_X] = radar_track.acceleration_covariance[RADAR_IDX::X_Y]; + acceleration_covariance[POSE_IDX::Y_Y] = radar_track.acceleration_covariance[RADAR_IDX::Y_Y]; + acceleration_covariance[POSE_IDX::Y_Z] = radar_track.acceleration_covariance[RADAR_IDX::Y_Z]; + acceleration_covariance[POSE_IDX::Z_X] = radar_track.acceleration_covariance[RADAR_IDX::X_Z]; + acceleration_covariance[POSE_IDX::Z_Y] = radar_track.acceleration_covariance[RADAR_IDX::Y_Z]; + acceleration_covariance[POSE_IDX::Z_Z] = radar_track.acceleration_covariance[RADAR_IDX::Z_Z]; + return acceleration_covariance; +} + uint8_t RadarTracksMsgsConverterNode::convertClassification(const uint16_t classification) { if (classification == static_cast(RadarTrackObjectID::UNKNOWN)) { 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/include/shape_estimation/corrector/bicycle_corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp new file mode 100644 index 0000000000000..ec1a2b8a42973 --- /dev/null +++ b/perception/shape_estimation/include/shape_estimation/corrector/bicycle_corrector.hpp @@ -0,0 +1,40 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ +#define SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ + +#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "utils.hpp" + +class BicycleCorrector : public VehicleCorrector +{ +public: + explicit BicycleCorrector(const bool use_reference_yaw = false) + : VehicleCorrector(use_reference_yaw) + { + corrector_utils::CorrectionBBParameters params; + params.min_width = 0.3; + params.max_width = 1.2; + params.default_width = (params.min_width + params.max_width) * 0.5; + params.min_length = 1.0; + params.max_length = 2.8; + params.default_length = (params.min_length + params.max_length) * 0.5; + setParams(params); + } + + ~BicycleCorrector() = default; +}; + +#endif // SHAPE_ESTIMATION__CORRECTOR__BICYCLE_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp index d52bdc21f916f..e4efc17181e52 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp @@ -15,6 +15,7 @@ #ifndef SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ #define SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ +#include "shape_estimation/corrector/bicycle_corrector.hpp" #include "shape_estimation/corrector/bus_corrector.hpp" #include "shape_estimation/corrector/car_corrector.hpp" #include "shape_estimation/corrector/corrector_interface.hpp" diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 8e90e3ea57cc0..f13b5d1e5f273 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -1,18 +1,13 @@ - - - - - + + + - - - - + diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index 9577b1e2d3197..e89eaac0a6db0 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -120,6 +120,8 @@ bool ShapeEstimator::applyCorrector( corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); } else if (label == Label::TRAILER) { corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); + } else if (label == Label::MOTORCYCLE || label == Label::BICYCLE) { + corrector_ptr.reset(new BicycleCorrector(use_reference_yaw)); } else { corrector_ptr.reset(new NoCorrector); } 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/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 4609638cc008c..9bcd03e8abfa1 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -1,60 +1,78 @@ # simple_object_merger -This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without data association algorithm. +This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without low calculation cost. ## Algorithm ### Background -This package can merge multiple DetectedObjects without matching processing. -[Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. -In addition, for now, [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. -To merge 6 DetectedObjects topics, 6 [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) nodes need to stand. +[Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) is mainly used for merge process with DetectedObjects. There are 2 characteristics in `Object_merger`. First, `object_merger` solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. Second, `object_merger` can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. To merge 6 DetectedObjects topics, 6 `object_merger` nodes need to stand for now. -So this package aim to merge DetectedObjects simply. -This package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. +So `simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. +The package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. + +### Use case + +Use case is as below. + +- Multiple radar detection + +`Simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics with `simple_object_merger`, the pipeline for faraway detection with radar is simpler. ### Limitation - Sensor data drops and delay -Merged objects will not be published until all topic data is received when initializing. -In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout. -When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects. -For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout. -The timeout parameter should be determined by sensor cycle time. +Merged objects will not be published until all topic data is received when initializing. In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout. When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects. For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout.The timeout parameter should be determined by sensor cycle time. - Post-processing -Because this package does not have matching processing, so it can be used only when post-processing is used. -For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. +Because this package does not have matching processing, there are overlapping objects depending on the input objects. So output objects can be used only when post-processing is used. For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing. -### Use case +## Interface -Use case is as below. +### Input -- Multiple radar detection +Input topics is defined by the parameter of `input_topics` (List[string]). The type of input topics is `std::vector`. + +### Output + +- `~/output/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`) + - Merged objects combined from input topics. + +### Parameters + +- `update_rate_hz` (double) [hz] + - Default parameter: 20.0 + +This parameter is update rate for the `onTimer` function. +This parameter should be same as the frame rate of input topics. -This package can be used for multiple radar detection. -Since [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) will be included later process in radar faraway detection, this package can be used. +- `new_frame_id` (string) + - Default parameter: "base_link" -## Input +This parameter is the header frame_id of the output topic. +If output topics use for perception module, it should be set for "base_link" -| Name | Type | Description | -| ---- | ------------------------------------------------------------------ | ------------------------------------------------------ | -| | std::vector | 3D detected objects. Topic names are set by parameters | +- `timeout_threshold` (double) [s] + - Default parameter: 0.1 -## Output +This parameter is the threshold for timeout judgement. +If the time difference between the first topic of `input_topics` and an input topic is exceeded to this parameter, then the objects of topic is not merged to output objects. -| Name | Type | Description | -| ------------------ | ----------------------------------------------------- | -------------- | -| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Merged objects | +```cpp + for (size_t i = 0; i < input_topic_size; i++) { + double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - + rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); + if (std::abs(time_diff) < node_param_.timeout_threshold) { + // merge objects + } + } +``` -## Parameters +- `input_topics` (List[string]) + - Default parameter: "[]" -| Name | Type | Description | Default value | -| :------------------ | :----------- | :----------------------------------- | :------------ | -| `update_rate_hz` | double | Update rate. [hz] | 20.0 | -| `new_frame_id` | string | The header frame_id of output topic. | "base_link" | -| `timeout_threshold` | double | Threshold for timeout judgement [s]. | 1.0 | -| `input_topics` | List[string] | Input topics name. | "[]" | +This parameter is the name of input topics. +For example, when this packages use for radar objects, `"[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/rear_left/detected_objects, /sensing/radar/rear_center/detected_objects, /sensing/radar/rear_right/detected_objects, /sensing/radar/front_right/detected_objects]"` can be set. +For now, the time difference is calculated by the header time between the first topic of `input_topics` and the input topics, so the most important objects to detect should be set in the first of `input_topics` list. diff --git a/perception/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/perception/tensorrt_yolo/schema/tensortt_yolo.json b/perception/tensorrt_yolo/schema/tensortt_yolo.json new file mode 100644 index 0000000000000..0b4724078c994 --- /dev/null +++ b/perception/tensorrt_yolo/schema/tensortt_yolo.json @@ -0,0 +1,108 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for tensorrt_yolo", + "type": "object", + "definitions": { + "tensorrt_yolo": { + "type": "object", + "properties": { + "num_anchors": { + "type": "number", + "default": [ + 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 + ], + "description": "The anchors to create bounding box candidates." + }, + "scale_x_y": { + "type": "number", + "default": [1.0, 1.0, 1.0], + "description": "scale parameter to eliminate grid sensitivity." + }, + "score_thresh": { + "type": "number", + "default": 0.1, + "description": "If the objectness score is less than this value, the object is ignored in yolo layer." + }, + "iou_thresh": { + "type": "number", + "default": 0.45, + "description": "The iou threshold for NMS method." + }, + "detections_per_im": { + "type": "number", + "default": 100, + "description": " The maximum detection number for one frame." + }, + "use_darknet_layer": { + "type": "boolean", + "default": true, + "description": "The flag to use yolo layer in darknet." + }, + "ignore_thresh": { + "type": "number", + "default": 0.5, + "description": "If the output score is less than this value, this object is ignored." + }, + "onnx_file": { + "type": "string", + "description": "The onnx file name for yolo model." + }, + "engine_file": { + "type": "string", + "description": "The tensorrt engine file name for yolo model." + }, + "label_file": { + "type": "string", + "description": "The label file with label names for detected objects written on it." + }, + "calib_image_directory": { + "type": "string", + "description": "The directory name including calibration images for int8 inference." + }, + "calib_cache_file": { + "type": "string", + "description": "The calibration cache file for int8 inference." + }, + "mode": { + "type": "string", + "default": "FP32", + "description": "The inference mode: FP32, FP16, INT8." + }, + "gpu_id": { + "type": "number", + "default": 0, + "description": "GPU device ID that runs the model." + } + }, + "required": [ + "num_anchors", + "scale_x_y", + "score_thresh", + "iou_thresh", + "detections_per_im", + "use_darknet_layer", + "ignore_thresh", + "onnx_file", + "engine_file", + "label_file", + "calib_image_directory", + "calib_cache_file", + "mode", + "gpu_id" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/tensorrt_yolo" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/tensorrt_yolo/src/nodelet.cpp b/perception/tensorrt_yolo/src/nodelet.cpp index dcb65114ad88f..fdcd8bf12db72 100644 --- a/perception/tensorrt_yolo/src/nodelet.cpp +++ b/perception/tensorrt_yolo/src/nodelet.cpp @@ -50,9 +50,9 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) std::string label_file = declare_parameter("label_file", ""); std::string calib_image_directory = declare_parameter("calib_image_directory", ""); std::string calib_cache_file = declare_parameter("calib_cache_file", ""); - std::string mode = declare_parameter("mode", "FP32"); - gpu_device_id_ = declare_parameter("gpu_id", 0); - yolo_config_.num_anchors = declare_parameter("num_anchors", 3); + std::string mode = declare_parameter("mode"); + gpu_device_id_ = declare_parameter("gpu_id"); + yolo_config_.num_anchors = declare_parameter("num_anchors"); auto anchors = declare_parameter( "anchors", std::vector{ 10, 13, 16, 30, 33, 23, 30, 61, 62, 45, 59, 119, 116, 90, 156, 198, 373, 326}); @@ -61,11 +61,11 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) auto scale_x_y = declare_parameter("scale_x_y", std::vector{1.0, 1.0, 1.0}); std::vector scale_x_y_float(scale_x_y.begin(), scale_x_y.end()); yolo_config_.scale_x_y = scale_x_y_float; - yolo_config_.score_thresh = declare_parameter("score_thresh", 0.1); - yolo_config_.iou_thresh = declare_parameter("iou_thresh", 0.45); - yolo_config_.detections_per_im = declare_parameter("detections_per_im", 100); - yolo_config_.use_darknet_layer = declare_parameter("use_darknet_layer", true); - yolo_config_.ignore_thresh = declare_parameter("ignore_thresh", 0.5); + yolo_config_.score_thresh = declare_parameter("score_thresh"); + yolo_config_.iou_thresh = declare_parameter("iou_thresh"); + yolo_config_.detections_per_im = declare_parameter("detections_per_im"); + yolo_config_.use_darknet_layer = declare_parameter("use_darknet_layer"); + yolo_config_.ignore_thresh = declare_parameter("ignore_thresh"); if (!yolo::set_cuda_device(gpu_device_id_)) { RCLCPP_ERROR(this->get_logger(), "Given GPU not exist or suitable"); diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index 5e46b9403462d..9c8e5a321d9be 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(autoware_cmake REQUIRED) +find_package(glog REQUIRED) autoware_package() @@ -31,6 +32,7 @@ ament_auto_add_library(decorative_tracker_merger_node SHARED target_link_libraries(decorative_tracker_merger_node Eigen3::Eigen + glog::glog ) rclcpp_components_register_node(decorative_tracker_merger_node diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp index bc6dfef9b80bf..951be5d45d508 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -18,6 +18,8 @@ #define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ // #include +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml index d74a8449b20e6..58184c090a1e1 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/tracking_object_merger/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs eigen + libgoogle-glog-dev mussp object_recognition_utils rclcpp diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index 47a333691eabf..d2bc643a8eec1 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -20,6 +20,8 @@ #include +#include + #include #include @@ -85,6 +87,10 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("decorative_object_merger_node"); + google::InstallFailureSignalHandler(); + // Subscriber sub_main_objects_ = create_subscription( "input/main_object", rclcpp::QoS{1}, diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 5987bdc2d1bef..81f63538f9e22 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -253,6 +253,68 @@ double mean(const double a, const double b) return (a + b) / 2.0; } +/** + * @brief compare two tracked objects motion direction is same or not + * + * @param main_obj + * @param sub_obj + * @return true + * @return false + */ +bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + // get yaw + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + // get velocity + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto main_vy = main_obj.kinematics.twist_with_covariance.twist.linear.y; + const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vy = sub_obj.kinematics.twist_with_covariance.twist.linear.y; + // calc velocity direction + const auto main_vyaw = std::atan2(main_vy, main_vx); + const auto sub_vyaw = std::atan2(sub_vy, sub_vx); + // get motion yaw angle + const auto main_motion_yaw = main_yaw + main_vyaw; + const auto sub_motion_yaw = sub_yaw + sub_vyaw; + // diff of motion yaw angle + const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw); + const auto normalized_motion_yaw_diff = + tier4_autoware_utils::normalizeRadian(motion_yaw_diff); // -pi ~ pi + // evaluate if motion yaw angle is same + constexpr double yaw_threshold = M_PI / 4.0; // 45 deg + if (std::abs(normalized_motion_yaw_diff) < yaw_threshold) { + return true; + } else { + return false; + } +} + +/** + * @brief compare two tracked objects yaw is reverted or not + * + * @param main_obj + * @param sub_obj + * @return true + * @return false + */ +bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + // get yaw + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + // calc yaw diff + const auto yaw_diff = std::fabs(main_yaw - sub_yaw); + const auto normalized_yaw_diff = tier4_autoware_utils::normalizeRadian(yaw_diff); // -pi ~ pi + // evaluate if yaw is reverted + constexpr double yaw_threshold = M_PI / 2.0; // 90 deg + if (std::abs(normalized_yaw_diff) >= yaw_threshold) { + return true; + } else { + return false; + } +} + // object kinematics merger // currently only support velocity fusion autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( @@ -261,18 +323,29 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; // copy main object at first output_kinematics = main_obj.kinematics; + auto sub_obj_ = sub_obj; + // do not merge reverse direction objects + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + return output_kinematics; + } else if (objectsYawIsReverted(main_obj, sub_obj)) { + // revert velocity (revert pose is not necessary because it is not used in tracking) + sub_obj_.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + sub_obj_.kinematics.twist_with_covariance.twist.linear.y *= -1.0; + } + + // currently only merge vx if (policy == MergePolicy::SKIP) { return output_kinematics; } else if (policy == MergePolicy::OVERWRITE) { output_kinematics.twist_with_covariance.twist.linear.x = - sub_obj.kinematics.twist_with_covariance.twist.linear.x; + sub_obj_.kinematics.twist_with_covariance.twist.linear.x; return output_kinematics; } else if (policy == MergePolicy::FUSION) { const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; - const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x; // inverse weight const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; - const auto sub_vx_cov = sub_obj.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vx_cov = sub_obj_.kinematics.twist_with_covariance.covariance[0]; double main_vx_weight, sub_vx_weight; if (main_vx_cov == 0.0) { main_vx_weight = 1.0 * 1e6; @@ -380,9 +453,21 @@ autoware_auto_perception_msgs::msg::Shape shapeMerger( void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) { - const auto vx_temp = main_obj.kinematics.twist_with_covariance.twist.linear.x; - main_obj = sub_obj; - main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + // do not update velocity + return; + } else if (objectsYawIsReverted(main_obj, sub_obj)) { + // revert velocity (revert pose is not necessary because it is not used in tracking) + const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = -vx_temp; + return; + } else { + // update velocity + const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; + } } void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) @@ -397,6 +482,12 @@ void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & su void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj) { + if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { + // warning + std::cerr << "[object_tracking_merger]: motion direction is different in " + "updateWholeTrackedObject function." + << std::endl; + } main_obj = sub_obj; } diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 2aecf66f8fb7b..758234f129f2a 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -52,10 +52,11 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| ----------------- | ---- | ------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | +| Name | Type | Description | +| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | if the value is `1`, cnn_classifier is used | +| `data_path` | str | packages data and artifacts directory path | +| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | ### Core Parameters diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp index e076ff5c69378..1281285ee53dd 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp @@ -66,6 +66,8 @@ class TrafficLightClassifierNodelet : public rclcpp::Node CNN = 1, }; + uint8_t classify_traffic_light_type_; + private: void connectCb(); @@ -86,6 +88,9 @@ class TrafficLightClassifierNodelet : public rclcpp::Node rclcpp::Publisher::SharedPtr traffic_signal_array_pub_; std::shared_ptr classifier_ptr_; + + double backlight_threshold_; + bool is_harsh_backlight(const cv::Mat & img) const; }; } // namespace traffic_light diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 10aa04cc585af..343531f4a00f1 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -11,8 +11,11 @@ + + + @@ -20,9 +23,11 @@ + + diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 90cca87245d22..1ebc14c4e6f93 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "traffic_light_classifier/nodelet.hpp" +#include + #include #include #include @@ -23,9 +25,13 @@ namespace traffic_light TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeOptions & options) : Node("traffic_light_classifier_node", options) { + classify_traffic_light_type_ = this->declare_parameter("classify_traffic_light_type", 0); + using std::placeholders::_1; using std::placeholders::_2; is_approximate_sync_ = this->declare_parameter("approximate_sync", false); + backlight_threshold_ = this->declare_parameter("backlight_threshold"); + if (is_approximate_sync_) { approximate_sync_.reset(new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, roi_sub_)); approximate_sync_->registerCallback( @@ -94,19 +100,73 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; + std::vector backlight_indices; for (size_t i = 0; i < input_rois_msg->rois.size(); i++) { - output_msg.signals[i].traffic_light_id = input_rois_msg->rois.at(i).traffic_light_id; + // skip if the roi is not detected + if (input_rois_msg->rois.at(i).roi.height == 0) { + break; + } + if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) { + continue; + } + output_msg.signals[images.size()].traffic_light_id = + input_rois_msg->rois.at(i).traffic_light_id; + output_msg.signals[images.size()].traffic_light_type = + input_rois_msg->rois.at(i).traffic_light_type; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; - images.emplace_back(cv_ptr->image, cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + + auto roi_img = cv_ptr->image(cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + if (is_harsh_backlight(roi_img)) { + backlight_indices.emplace_back(i); + } + images.emplace_back(roi_img); } + + output_msg.signals.resize(images.size()); if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; } + + // append the undetected rois as unknown + for (const auto & input_roi : input_rois_msg->rois) { + if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) { + tier4_perception_msgs::msg::TrafficSignal tlr_sig; + tlr_sig.traffic_light_id = input_roi.traffic_light_id; + tlr_sig.traffic_light_type = input_roi.traffic_light_type; + tier4_perception_msgs::msg::TrafficLightElement element; + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::CIRCLE; + element.confidence = 0.0; + tlr_sig.elements.push_back(element); + output_msg.signals.push_back(tlr_sig); + } + } + + for (const auto & idx : backlight_indices) { + auto & elements = output_msg.signals.at(idx).elements; + for (auto & element : elements) { + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.confidence = 0.0; + } + } + output_msg.header = input_image_msg->header; traffic_signal_array_pub_->publish(output_msg); } +bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) const +{ + cv::Mat y_cr_cb; + cv::cvtColor(img, y_cr_cb, cv::COLOR_RGB2YCrCb); + + const cv::Scalar mean_values = cv::mean(y_cr_cb); + const double intensity = (mean_values[0] - 112.5) / 112.5; + + return backlight_threshold_ <= intensity; +} + } // namespace traffic_light #include diff --git a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp index f2f1f17be6894..5c89c76a11833 100644 --- a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp +++ b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp @@ -115,6 +115,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node void detectionMatch( std::map & id2expectRoi, std::map & id2detections, TrafficLightRoiArray & out_rois); + /** * @brief convert image message to cv::Mat * @@ -137,7 +138,8 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node * @return true succeed * @return false failed */ - bool readLabelFile(const std::string & filepath, int & tlr_id, int & num_class); + bool readLabelFile( + const std::string & filepath, std::vector & tlr_label_id_, int & num_class); // variables image_transport::SubscriberFilter image_sub_; @@ -162,7 +164,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node bool is_approximate_sync_; double score_thresh_; - int tlr_id_; + std::vector tlr_label_id_; int batch_size_; std::unique_ptr trt_yolox_; diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 86e77bc31cd71..8037dc5472fbe 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -73,7 +73,7 @@ TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet( float nms_threshold = declare_parameter("fine_detector_nms_thresh", 0.65); is_approximate_sync_ = this->declare_parameter("approximate_sync", false); - if (!readLabelFile(label_path, tlr_id_, num_class)) { + if (!readLabelFile(label_path, tlr_label_id_, num_class)) { RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); } @@ -157,6 +157,7 @@ void TrafficLightFineDetectorNodelet::callback( tensorrt_yolox::ObjectArrays inference_results; std::vector lts; std::vector roi_ids; + for (size_t roi_i = 0; roi_i < rough_roi_msg->rois.size(); roi_i++) { const auto & rough_roi = rough_roi_msg->rois[roi_i]; cv::Point lt(rough_roi.roi.x_offset, rough_roi.roi.y_offset); @@ -178,7 +179,7 @@ void TrafficLightFineDetectorNodelet::callback( trt_yolox_->doMultiScaleInference(original_image, inference_results, rois); for (size_t batch_i = 0; batch_i < true_batch_size; batch_i++) { for (const tensorrt_yolox::Object & detection : inference_results[batch_i]) { - if (detection.score < score_thresh_ || detection.type != tlr_id_) { + if (detection.score < score_thresh_) { continue; } cv::Point lt_roi( @@ -190,6 +191,7 @@ void TrafficLightFineDetectorNodelet::callback( det.y_offset = lt_roi.y; det.width = rb_roi.x - lt_roi.x; det.height = rb_roi.y - lt_roi.y; + det.type = detection.type; id2detections[roi_ids[batch_i]].push_back(det); } } @@ -270,13 +272,28 @@ void TrafficLightFineDetectorNodelet::detectionMatch( } out_rois.rois.clear(); - for (const auto & p : bestDetections) { + std::vector invalid_roi_id; + for (const auto & [tlr_id, roi] : id2expectRoi) { + // if matches, update the roi info + if (!bestDetections.count(tlr_id)) { + invalid_roi_id.emplace_back(tlr_id); + continue; + } + TrafficLightRoi tlr; + tlr.traffic_light_id = tlr_id; + const auto & object = bestDetections.at(tlr_id); + tlr.traffic_light_type = roi.traffic_light_type; + tlr.roi.x_offset = object.x_offset; + tlr.roi.y_offset = object.y_offset; + tlr.roi.width = object.width; + tlr.roi.height = object.height; + out_rois.rois.push_back(tlr); + } + // append undetected rois at the end + for (const auto & id : invalid_roi_id) { TrafficLightRoi tlr; - tlr.traffic_light_id = p.first; - tlr.roi.x_offset = p.second.x_offset; - tlr.roi.y_offset = p.second.y_offset; - tlr.roi.width = p.second.width; - tlr.roi.height = p.second.height; + tlr.traffic_light_id = id; + tlr.traffic_light_type = id2expectRoi[id].traffic_light_type; out_rois.rois.push_back(tlr); } } @@ -318,9 +335,8 @@ bool TrafficLightFineDetectorNodelet::fitInFrame( } bool TrafficLightFineDetectorNodelet::readLabelFile( - const std::string & filepath, int & tlr_id, int & num_class) + const std::string & filepath, std::vector & tlr_label_id_, int & num_class) { - tlr_id = -1; std::ifstream labelsFile(filepath); if (!labelsFile.is_open()) { RCLCPP_ERROR(this->get_logger(), "Could not open label file. [%s]", filepath.c_str()); @@ -329,13 +345,13 @@ bool TrafficLightFineDetectorNodelet::readLabelFile( std::string label; int idx = 0; while (getline(labelsFile, label)) { - if (label == "traffic_light") { - tlr_id = idx; + if (label == "traffic_light" || label == "pedestrian_traffic_light") { + tlr_label_id_.push_back(idx); } idx++; } num_class = idx; - return tlr_id != -1; + return tlr_label_id_.size() != 0; } } // namespace traffic_light diff --git a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml index 2b8ff4aa737b7..2eb671c1f5150 100644 --- a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml +++ b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml @@ -6,3 +6,5 @@ max_vibration_width: 0.5 # -0.25 ~ 0.25 m max_vibration_depth: 0.5 # -0.25 ~ 0.25 m max_detection_range: 200.0 + car_traffic_light_max_angle_range: 40.0 + pedestrian_traffic_light_max_angle_range: 80.0 diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index e46431a7b199e..0d2b7ffeb7597 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -58,6 +58,8 @@ class MapBasedDetector : public rclcpp::Node double max_timestamp_offset; double timestamp_sample_len; double max_detection_range; + double car_traffic_light_max_angle_range; + double pedestrian_traffic_light_max_angle_range; }; struct IdLessThan @@ -93,10 +95,14 @@ class MapBasedDetector : public rclcpp::Node std::shared_ptr all_traffic_lights_ptr_; std::shared_ptr route_traffic_lights_ptr_; + std::set pedestrian_tl_id_; + lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + std::shared_ptr overall_graphs_ptr_; + Config config_; /** * @brief Calculated the transform from map to frame_id at timestamp t diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index f0a5d7b7b1fde..5689e55dca316 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -137,6 +137,10 @@ MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options) config_.max_timestamp_offset = declare_parameter("max_timestamp_offset", 0.0); config_.timestamp_sample_len = declare_parameter("timestamp_sample_len", 0.01); config_.max_detection_range = declare_parameter("max_detection_range", 200.0); + config_.car_traffic_light_max_angle_range = + declare_parameter("car_traffic_light_max_angle_range", 40.0); + config_.pedestrian_traffic_light_max_angle_range = + declare_parameter("pedestrian_traffic_light_max_angle_range", 80.0); if (config_.max_detection_range <= 0) { RCLCPP_ERROR_STREAM( @@ -287,6 +291,11 @@ bool MapBasedDetector::getTrafficLightRoi( tier4_perception_msgs::msg::TrafficLightRoi & roi) const { roi.traffic_light_id = traffic_light.id(); + if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { + roi.traffic_light_type = tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT; + } else { + roi.traffic_light_type = tier4_perception_msgs::msg::TrafficLightRoi::CAR_TRAFFIC_LIGHT; + } // for roi.x_offset and roi.y_offset { @@ -392,7 +401,6 @@ void MapBasedDetector::mapCallback( for (auto tl_itr = all_lanelet_traffic_lights.begin(); tl_itr != all_lanelet_traffic_lights.end(); ++tl_itr) { lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; - auto lights = tl->trafficLights(); for (auto lsp : lights) { if (!lsp.isLineString()) { // traffic lights must be linestrings @@ -401,6 +409,28 @@ void MapBasedDetector::mapCallback( all_traffic_lights_ptr_->insert(static_cast(lsp)); } } + + auto crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); + for (const auto & tl : lanelet::utils::query::autowareTrafficLights(crosswalk_lanelets)) { + for (const auto & lsp : tl->trafficLights()) { + if (lsp.isLineString()) { // traffic lights must be linestrings + pedestrian_tl_id_.insert(lsp.id()); + } + } + } + + // crosswalk + const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + lanelet::routing::RoutingGraphConstPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + overall_graphs_ptr_ = + std::make_shared(overall_graphs); } void MapBasedDetector::routeCallback( @@ -436,6 +466,34 @@ void MapBasedDetector::routeCallback( route_traffic_lights_ptr_->insert(static_cast(lsp)); } } + + // crosswalk trafficlights + lanelet::ConstLanelets conflicting_crosswalks; + pedestrian_tl_id_.clear(); + + for (const auto & route_lanelet : route_lanelets) { + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflict_lls = + overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID); + for (const auto & lanelet : conflict_lls) { + conflicting_crosswalks.push_back(lanelet); + } + } + std::vector crosswalk_lanelet_traffic_lights = + lanelet::utils::query::autowareTrafficLights(conflicting_crosswalks); + for (auto tl_itr = crosswalk_lanelet_traffic_lights.begin(); + tl_itr != crosswalk_lanelet_traffic_lights.end(); ++tl_itr) { + lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; + + auto lights = tl->trafficLights(); + for (auto lsp : lights) { + if (!lsp.isLineString()) { // traffic lights must be linestrings + continue; + } + route_traffic_lights_ptr_->insert(static_cast(lsp)); + pedestrian_tl_id_.insert(lsp.id()); + } + } } void MapBasedDetector::getVisibleTrafficLights( @@ -445,12 +503,19 @@ void MapBasedDetector::getVisibleTrafficLights( std::vector & visible_traffic_lights) const { for (const auto & traffic_light : all_traffic_lights) { - // some "Traffic Light" are actually not traffic lights if ( traffic_light.hasAttribute("subtype") == false || traffic_light.attribute("subtype").value() == "solid") { continue; } + // set different max angle range for ped and car traffic light + double max_angle_range; + if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { + max_angle_range = + tier4_autoware_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); + } else { + max_angle_range = tier4_autoware_utils::deg2rad(config_.car_traffic_light_max_angle_range); + } // traffic light bottom left const auto & tl_bl = traffic_light.front(); // traffic light bottom right @@ -467,7 +532,6 @@ void MapBasedDetector::getVisibleTrafficLights( // check angle range const double tl_yaw = tier4_autoware_utils::normalizeRadian( std::atan2(tl_br.y() - tl_bl.y(), tl_br.x() - tl_bl.x()) + M_PI_2); - constexpr double max_angle_range = tier4_autoware_utils::deg2rad(40.0); // get direction of z axis tf2::Vector3 camera_z_dir(0, 0, 1); diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp index 71930c65f88c9..ae6ba8c1feedb 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace traffic_light { @@ -71,7 +72,8 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node const tier4_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr in_signal_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg, + const uint8_t traffic_light_type); rclcpp::Subscription::SharedPtr map_sub_; /** @@ -89,7 +91,6 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node * */ std::shared_ptr cloud_occlusion_predictor_; - typedef perception_utils::PrimeSynchronizer< tier4_perception_msgs::msg::TrafficSignalArray, tier4_perception_msgs::msg::TrafficLightRoiArray, sensor_msgs::msg::CameraInfo, @@ -97,6 +98,11 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node SynchronizerType; std::shared_ptr synchronizer_; + std::shared_ptr synchronizer_ped_; + + std::vector subscribed_; + std::vector occlusion_ratios_; + tier4_perception_msgs::msg::TrafficSignalArray out_msg_; }; } // namespace traffic_light #endif // TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__NODELET_HPP_ diff --git a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml b/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml index 795267b920e24..aa7e34f8a4a09 100644 --- a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml +++ b/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml @@ -4,7 +4,8 @@ - + + @@ -13,7 +14,8 @@ - + + diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index c460f6a623bc4..6429640ff44c1 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -57,6 +58,7 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( // publishers signal_pub_ = create_publisher("~/output/traffic_signals", 1); + // configuration parameters config_.azimuth_occlusion_resolution_deg = declare_parameter("azimuth_occlusion_resolution_deg", 0.15); @@ -72,12 +74,26 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( config_.elevation_occlusion_resolution_deg); const std::vector topics{ - "~/input/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; + "~/input/car/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; const std::vector qos(topics.size(), rclcpp::SensorDataQoS()); synchronizer_ = std::make_shared( this, topics, qos, - std::bind(&TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4), + std::bind( + &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + tier4_perception_msgs::msg::TrafficLightRoi::CAR_TRAFFIC_LIGHT), + config_.max_image_cloud_delay, config_.max_wait_t); + + const std::vector topics_ped{ + "~/input/pedestrian/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; + const std::vector qos_ped(topics_ped.size(), rclcpp::SensorDataQoS()); + synchronizer_ped_ = std::make_shared( + this, topics_ped, qos_ped, + std::bind( + &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT), config_.max_image_cloud_delay, config_.max_wait_t); + + subscribed_.resize(2, false); } void TrafficLightOcclusionPredictorNodelet::mapCallback( @@ -109,27 +125,65 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const tier4_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr in_signal_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg) + const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg, + const uint8_t traffic_light_type) { - tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; std::vector occlusion_ratios; - if ( - in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr || - in_roi_msg->rois.size() != in_signal_msg->signals.size()) { - occlusion_ratios.resize(out_msg.signals.size(), 0); + size_t not_detected_roi = 0; + if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { + occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { - cloud_occlusion_predictor_->predict( - in_cam_info_msg, in_roi_msg, in_cloud_msg, tf_buffer_, traffic_light_position_map_, - occlusion_ratios); + tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; + selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); + for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { + // not detected roi + if (in_roi_msg->rois[i].roi.height == 0) { + not_detected_roi++; + continue; + } + if (in_roi_msg->rois.at(i).traffic_light_type == traffic_light_type) { + selected_roi_msg.rois.push_back(in_roi_msg->rois.at(i)); + } + } + + tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; + + if (selected_roi_msg.rois.size() != in_signal_msg->signals.size() - not_detected_roi) { + occlusion_ratios.resize(in_signal_msg->signals.size(), 0); + } else { + auto selected_roi_msg_ptr = + std::make_shared(selected_roi_msg); + cloud_occlusion_predictor_->predict( + in_cam_info_msg, selected_roi_msg_ptr, in_cloud_msg, tf_buffer_, + traffic_light_position_map_, occlusion_ratios); + } } + + size_t predicted_num = out_msg_.signals.size(); + for (size_t i = 0; i < occlusion_ratios.size(); i++) { + out_msg_.signals.push_back(in_signal_msg->signals.at(i)); + if (occlusion_ratios[i] >= config_.max_occlusion_ratio) { - traffic_light_utils::setSignalUnknown(out_msg.signals[i]); + traffic_light_utils::setSignalUnknown(out_msg_.signals.at(predicted_num + i), 0.0); } } - signal_pub_->publish(out_msg); -} + // push back not detected rois + for (size_t i = occlusion_ratios.size(); i < in_signal_msg->signals.size(); ++i) { + out_msg_.signals.push_back(in_signal_msg->signals[i]); + } + + subscribed_.at(traffic_light_type) = true; + + if (std::all_of(subscribed_.begin(), subscribed_.end(), [](bool v) { return v; })) { + auto pub_msg = std::make_unique(out_msg_); + pub_msg->header = in_signal_msg->header; + signal_pub_->publish(std::move(pub_msg)); + out_msg_.signals.clear(); + std::fill(subscribed_.begin(), subscribed_.end(), false); + } +} } // namespace traffic_light #include diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 7daa0849abe5e..6f96913420bad 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -88,6 +88,10 @@ void CloudOcclusionPredictor::predict( image_geometry::PinholeCameraModel pinhole_model; pinhole_model.fromCameraInfo(*camera_info_msg); for (size_t i = 0; i < rois_msg->rois.size(); i++) { + // skip if no detection + if (rois_msg->rois[i].roi.height == 0) { + continue; + } calcRoiVector3D( rois_msg->rois[i], pinhole_model, traffic_light_position_map, tf_camera2map, roi_tls[i], roi_brs[i]); @@ -107,7 +111,7 @@ void CloudOcclusionPredictor::predict( lidar_rays_[static_cast(ray.azimuth)][static_cast(ray.elevation)].push_back(ray); } for (size_t i = 0; i < roi_tls.size(); i++) { - occlusion_ratios[i] = predict(roi_tls[i], roi_brs[i]); + occlusion_ratios[i] = rois_msg->rois[i].roi.height == 0 ? 0 : predict(roi_tls[i], roi_brs[i]); } } diff --git a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp similarity index 88% rename from perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp rename to perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp index 3f83cf6e926ad..5e981bbeafda1 100644 --- a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_map_visualizer/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ -#define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#ifndef TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ #include #include @@ -49,4 +49,4 @@ class TrafficLightMapVisualizerNode : public rclcpp::Node } // namespace traffic_light -#endif // TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ +#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ diff --git a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp similarity index 95% rename from perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp rename to perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp index 1c64eb2f9e82b..13d9045e89e98 100644 --- a/perception/traffic_light_visualization/include/traffic_light_roi_visualizer/nodelet.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ -#define TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#ifndef TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#define TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ #include #include @@ -123,4 +123,4 @@ class TrafficLightRoiVisualizerNodelet : public rclcpp::Node } // namespace traffic_light -#endif // TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ +#endif // TRAFFIC_LIGHT_VISUALIZATION__TRAFFIC_LIGHT_ROI_VISUALIZER__NODELET_HPP_ diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp index 02f7a67302945..62d416457c6f0 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/main.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" + #include -#include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index 53e06e47a1873..e621a20450bbf 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_map_visualizer/node.hpp" + #include #include #include -#include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index 2f2d80ba43233..4ff9729695be2 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp" // NOLINT(whitespace/line_length) + #include #include -#include #include #include diff --git a/planning/.pages b/planning/.pages index 7cbd36d2ca2d0..e244f1d1ea25d 100644 --- a/planning/.pages +++ b/planning/.pages @@ -5,18 +5,18 @@ nav: - 'Concept': - 'Planner Manager': planning/behavior_path_planner/docs/behavior_path_planner_manager_design - 'Scene Module Interface': planning/behavior_path_planner/docs/behavior_path_planner_interface_design - - 'Path Generation': planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design - - 'Collision Assessment/Safety Check': planning/behavior_path_planner/docs/behavior_path_planner_safety_check - - 'Dynamic Drivable Area': planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design - - 'Turn Signal': planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design + - 'Path Generation': planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design + - 'Collision Assessment/Safety Check': planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check + - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design + - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design - - 'Avoidance by Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design - - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design - - 'Goal Planner': planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design - - 'Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design - - 'Side Shift': planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design - - 'Start Planner': planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design + - 'Avoidance': planning/behavior_path_avoidance_module + - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module + - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module + - 'Goal Planner': planning/behavior_path_goal_planner_module + - 'Lane Change': planning/behavior_path_lane_change_module + - 'Side Shift': planning/behavior_path_side_shift_module + - 'Start Planner': planning/behavior_path_start_planner_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/behavior_velocity_planner - 'Template for Custom Module': planning/behavior_velocity_template_module diff --git a/planning/README.md b/planning/README.md index 4f13bac3392a0..0302d37a1180b 100644 --- a/planning/README.md +++ b/planning/README.md @@ -43,7 +43,7 @@ There are multiple parameters available for configuration, and users have the op This guide outlines the steps for integrating your custom module into Autoware: -- Add your modules to the `default_present.yaml` file. For example +- Add your modules to the `default_preset.yaml` file. For example ```yaml - arg: diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md b/planning/behavior_path_avoidance_by_lane_change_module/README.md similarity index 86% rename from planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md rename to planning/behavior_path_avoidance_by_lane_change_module/README.md index 2c91cc43995ac..d91d7116ee056 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md +++ b/planning/behavior_path_avoidance_by_lane_change_module/README.md @@ -9,11 +9,11 @@ This module is designed as one of the obstacle avoidance features and generates - Exist lane changeable lanelet. - Exist avoidance target objects on ego driving lane. -![avoidance_by_lane_change](../image/avoidance_by_lane_change/avoidance_by_lane_change.svg) +![avoidance_by_lane_change](./images/avoidance_by_lane_change.svg) ## Inner-workings / Algorithms -Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](./behavior_path_planner_avoidance_design.md) and the path generation logic of the [Normal Lane Change Module](./behavior_path_planner_lane_change_design.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](../behavior_path_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. Check that the following conditions are satisfied after the filtering process for the avoidance target. @@ -21,7 +21,7 @@ Check that the following conditions are satisfied after the filtering process fo This module is launched when the number of avoidance target objects on **EGO DRIVING LANE** is greater than `execute_object_num`. If there are no avoidance targets in the ego driving lane or their number is less than the parameter, the obstacle is avoided by normal avoidance behavior (if the normal avoidance module is registered). -![trigger_1](../image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg) +![trigger_1](./images/avoidance_by_lc_trigger_1.svg) ### Lane change end point condition @@ -29,7 +29,7 @@ Unlike the normal avoidance module, which specifies the shift line end point, th Although setting the parameter to `false` would increase the scene of avoidance by lane change, it is assumed that sufficient lateral margin may not be ensured in some cases because the vehicle passes by the side of obstacles during the lane change. -![trigger_2](../image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg) +![trigger_2](./images/avoidance_by_lc_trigger_2.svg) ## Parameters 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 91% 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 index 9518185d30d63..3e8907cdccdf6 100644 --- 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 @@ -85,3 +85,11 @@ bicycle: false # [-] motorcycle: false # [-] pedestrian: false # [-] + + constraints: + # lateral constraints + lateral: + velocity: [1.0, 1.38, 11.1] # [m/s] + max_accel_values: [0.5, 0.5, 0.5] # [m/ss] + min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lane_change.svg b/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lane_change.svg rename to planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg b/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg rename to planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg b/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg rename to planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp index e177244930da6..8e7d1f67d3157 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -16,8 +16,6 @@ #include "behavior_path_avoidance_module/data_structs.hpp" -#include - namespace behavior_path_planner { struct AvoidanceByLCParameters : public AvoidanceParameters @@ -27,6 +25,10 @@ struct AvoidanceByLCParameters : public AvoidanceParameters // execute only when lane change end point is before the object. bool execute_only_when_lane_change_finish_before_object{false}; + + explicit AvoidanceByLCParameters(const AvoidanceParameters & param) : AvoidanceParameters(param) + { + } }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp index 0bc08ccd400ca..897956a392008 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp @@ -17,7 +17,7 @@ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/interface.hpp" #include @@ -40,6 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface bool isExecutionRequested() const override; + void processOnEntry() override; + protected: void updateRTCStatus(const double start_distance, const double finish_distance) override; }; 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 b1beae9c6fc03..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 @@ -17,7 +17,7 @@ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_avoidance_by_lane_change_module/interface.hpp" -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_lane_change_module/manager.hpp" #include @@ -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/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 1e667c207c64f..7a82ba60a1eaa 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -16,13 +16,15 @@ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_avoidance_module/helper.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include namespace behavior_path_planner { using AvoidanceDebugData = DebugData; +using helper::avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { @@ -46,6 +48,7 @@ class AvoidanceByLaneChange : public NormalLaneChange ObjectDataArray registered_objects_; mutable ObjectDataArray stopped_objects_; + std::shared_ptr avoidance_helper_; ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index da51dd9f235dc..0f9f3f6dc9d42 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -19,8 +19,10 @@ eigen3_cmake_module behavior_path_avoidance_module + behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common + lanelet2_extension motion_utils pluginlib rclcpp diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 2e451461abab7..efdb302a58ac2 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -45,7 +45,11 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { - return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); + return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck(); +} +void AvoidanceByLaneChangeInterface::processOnEntry() +{ + waitApproval(); } void AvoidanceByLaneChangeInterface::updateRTCStatus( 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 61b0a36ae0fb0..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 @@ -14,6 +14,8 @@ #include "behavior_path_avoidance_by_lane_change_module/manager.hpp" +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -35,9 +37,11 @@ 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); - AvoidanceByLCParameters p{}; // unique parameters { const std::string ns = "avoidance_by_lane_change."; @@ -139,6 +143,18 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + } + { const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); 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 bfa873d9518ef..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 @@ -15,8 +15,13 @@ #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.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/utils.hpp" + +#include +#include #include #include @@ -25,11 +30,22 @@ namespace behavior_path_planner { +namespace +{ +lanelet::BasicLineString3d toLineString3d(const std::vector & bound) +{ + lanelet::BasicLineString3d ret{}; + std::for_each( + bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); + return ret; +} +} // namespace AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) : NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), - avoidance_parameters_(std::move(avoidance_parameters)) + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_helper_{std::make_shared(avoidance_parameters_)} { } @@ -37,48 +53,69 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const { const auto & data = avoidance_data_; - if (!status_.is_safe) { + if (data.target_objects.empty()) { + RCLCPP_DEBUG(logger_, "no empty objects"); return false; } const auto & object_parameters = avoidance_parameters_->object_parameters; - const auto is_more_than_threshold = - std::any_of(object_parameters.begin(), object_parameters.end(), [&](const auto & p) { - const auto & objects = avoidance_data_.target_objects; - - const size_t num = std::count_if(objects.begin(), objects.end(), [&p](const auto & object) { - const auto target_class = - utils::getHighestProbLabel(object.object.classification) == p.first; - return target_class && object.avoid_required; - }); - return num >= p.second.execute_num; - }); + const auto count_target_object = [&](const auto sum, const auto & p) { + const auto & objects = avoidance_data_.target_objects; - if (!is_more_than_threshold) { - return false; - } + const auto is_avoidance_target = [&p](const auto & object) { + const auto target_class = utils::getHighestProbLabel(object.object.classification) == p.first; + return target_class && object.avoid_required; + }; - const auto & front_object = data.target_objects.front(); + return sum + std::count_if(objects.begin(), objects.end(), is_avoidance_target); + }; + const auto num_of_avoidance_targets = + std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object); - const auto THRESHOLD = avoidance_parameters_->execute_object_longitudinal_margin; - if (front_object.longitudinal < THRESHOLD) { + if (num_of_avoidance_targets < 1) { + RCLCPP_DEBUG(logger_, "no avoidance target"); return false; } - const auto path = status_.lane_change_path.path; - const auto to_lc_end = motion_utils::calcSignedArcLength( - status_.lane_change_path.path.points, getEgoPose().position, - status_.lane_change_path.info.shift_line.end.position); - const auto execute_only_when_lane_change_finish_before_object = - avoidance_parameters_->execute_only_when_lane_change_finish_before_object; - const auto not_enough_distance = front_object.longitudinal < to_lc_end; - - if (execute_only_when_lane_change_finish_before_object && not_enough_distance) { + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "no empty lanes"); return false; } - return true; + const auto & nearest_object = data.target_objects.front(); + + // get minimum lane change distance + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, + lane_change_parameters_->backward_length_buffer_for_end_of_lane); + + // get minimum avoid distance + + const auto ego_width = getCommonParam().vehicle_width; + const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); + const auto nearest_object_parameter = + avoidance_parameters_->object_parameters.at(nearest_object_type); + const auto avoid_margin = + nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + + nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + + avoidance_helper_->setData(planner_data_); + const auto shift_length = avoidance_helper_->getShiftLength( + nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + + const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length); + + RCLCPP_DEBUG( + logger_, + "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance " + "%.3f", + nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance); + + return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance); } bool AvoidanceByLaneChange::specialExpiredCheck() const @@ -125,6 +162,23 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( data.current_lanelets = getCurrentLanes(); + // expand drivable lanes + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + lanelet, planner_data_, avoidance_parameters_, false)); + }); + + // calc drivable bound + const auto shorten_lanes = + utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); + data.left_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, + avoidance_parameters_->use_intersection_areas, true)); + data.right_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, + avoidance_parameters_->use_intersection_areas, false)); + // get related objects from dynamic_objects, and then separates them as target objects and non // target objects fillAvoidanceTargetObjects(data, debug); @@ -133,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_); @@ -173,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( @@ -182,6 +238,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( using boost::geometry::return_centroid; using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcLateralDeviation; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -213,8 +270,11 @@ ObjectData AvoidanceByLaneChange::createObjectData( utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); // Calc lateral deviation from path to target object. - object_data.lateral = - tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position); + 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( 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 2264669fdcdbe..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 @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include @@ -47,6 +48,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); @@ -63,12 +66,11 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_planner") + - "/config/lane_change/lane_change.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", 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_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_avoidance_module/README.md similarity index 96% rename from planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md rename to planning/behavior_path_avoidance_module/README.md index c4580bb1e82c3..122ad7adcce9e 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -20,7 +20,7 @@ This module executes avoidance over lanes, and the decision requires the lane st The following figure shows a simple explanation of the logic for avoidance path generation. First, target objects are picked up, and shift requests are generated for each object. These shift requests are generated by taking into account the lateral jerk required for avoidance (red lines). Then these requests are merged and the shift points are created on the reference path (blue line). Filtering operations are performed on the shift points such as removing unnecessary shift points (yellow line), and finally a smooth avoidance path is generated by combining Clothoid-like curve primitives (green line). -![fig1](../image/avoidance/avoidance_design.fig1.drawio.svg) +![fig1](./images/avoidance_design.fig1.drawio.svg) ### Flowchart @@ -185,7 +185,7 @@ The avoidance target should be limited to stationary objects (you should not avo - This means that the vehicle is parked on the edge of the lane. This prevents the vehicle from avoiding a vehicle waiting at a traffic light in the middle of the lane. However, this is not an appropriate implementation for the purpose. Even if a vehicle is in the center of the lane, it should be avoided if it has its hazard lights on, and this is a point that should be improved in the future as the recognition performance improves. - Object is not behind ego(default: > -`2.0 m`) or too far(default: < `150.0 m`) and object is not behind the path goal. -![fig1](../image/avoidance/target_vehicle_selection.svg) +![fig1](./images/target_vehicle_selection.svg) ### Parked-car detection @@ -203,7 +203,7 @@ $$ The closer the object is to the shoulder, the larger the value of $ratio$ (theoretical max value is 1.0), and it compares the value and `object_check_shiftable_ratio` to determine whether the object is a parked-car. If the road has no road shoulders, it uses `object_check_min_road_shoulder_width` as a road shoulder width virtually. -![fig2](../image/avoidance/parked-car-detection.svg) +![fig2](./images/parked-car-detection.svg) ### Compensation for detection lost @@ -365,9 +365,9 @@ Therefore, in order to reduce the influence of the noise, avoidance module gener object_envelope_buffer: 0.3 # [m] ``` -![fig1](../image/avoidance/envelope_polygon.svg) +![fig1](./images/envelope_polygon.svg) -![fig1](../image/avoidance/shift_line_generation.svg) +![fig1](./images/shift_line_generation.svg) ### Computing Shift Length and Shift Points @@ -388,7 +388,7 @@ else The following figure illustrates these variables(This figure just shows the max value of lateral shift length). -![shift_point_and_its_constraints](../image/avoidance/avoidance_module-shift_point_and_its_constraints.drawio.png) +![shift_point_and_its_constraints](./images/avoidance_module-shift_point_and_its_constraints.drawio.png) ### Rationale of having safety buffer and safety margin @@ -399,7 +399,7 @@ To compute the shift length, additional parameters that can be tune are `lateral - It is recommended to set the value to more than half of the ego vehicle's width. - The `road_shoulder_safety_margin` will prevent the module from generating a path that might cause the vehicle to go too near the road shoulder or adjacent lane dividing line. -![shift_length_parameters](../image/shift_length_parameters.drawio.svg) +![shift_length_parameters](./images/shift_length_parameters.drawio.svg) ### Generating path only within lanelet boundaries @@ -410,7 +410,7 @@ The shift length is set as a constant value before the feature is implemented. S These elements are used to compute the distance from the object to the road's shoulder (`to_road_shoulder_distance`). The parameters `use_adjacent_lane` and `use_opposite_lane` allows further configuration of the to `to_road_shoulder_distance`. The following image illustrates the configuration. -![obstacle_to_road_shoulder_distance](../image/avoidance/obstacle_to_road_shoulder_distance.drawio.svg) +![obstacle_to_road_shoulder_distance](./images/obstacle_to_road_shoulder_distance.drawio.svg) If one of the following conditions is `false`, then the shift point will not be generated. @@ -565,7 +565,7 @@ To solve that, this module provides a parameter for the minimum avoidance speed, - If the ego vehicle speed is lower than "nominal" minimum speed, use the minimum speed in the calculation of the jerk. - If the ego vehicle speed is lower than "sharp" minimum speed and a nominal lateral jerk is not enough for avoidance (the case where the ego vehicle is stopped close to the obstacle), use the "sharp" minimum speed in the calculation of the jerk (it should be lower than "nominal" speed). -![fig1](../image/avoidance/how_to_decide_path_shape_one_object.drawio.svg) +![fig1](./images/how_to_decide_path_shape_one_object.drawio.svg) #### Multiple obstacle case (one direction) @@ -575,13 +575,13 @@ Generate shift points for multiple obstacles. All of them are merged to generate For the details of the shift point filtering, see [filtering for shift points](#filtering-for-shift-points). -![fig1](../image/avoidance/how_to_decide_path_shape_multi_object_one_direction.drawio.svg) +![fig1](./images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg) #### Multiple obstacle case (both direction) Generate shift points for multiple obstacles. All of them are merged to generate new shift points. If there are areas where the desired shifts conflict in different directions, the sum of the maximum shift amounts of these areas is used as the final shift amount. The rest of the process is the same as in the case of one direction. -![fig1](../image/avoidance/how_to_decide_path_shape_multi_object_both_direction.drawio.svg) +![fig1](./images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg) #### Filtering for shift points @@ -608,23 +608,23 @@ use_hatched_road_markings: false #### adjacent lane -![fig1](../image/avoidance/use_adjacent_lane.svg) +![fig1](./images/use_adjacent_lane.svg) #### opposite lane -![fig1](../image/avoidance/use_opposite_lane.svg) +![fig1](./images/use_opposite_lane.svg) #### intersection areas The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) -![fig1](../image/avoidance/use_intersection_areas.svg) +![fig1](./images/use_intersection_areas.svg) #### hatched road markings The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) -![fig1](../image/avoidance/use_hatched_road_markings.svg) +![fig1](./images/use_hatched_road_markings.svg) ### Safety check @@ -647,11 +647,11 @@ safety_check_backward_distance: 50.0 # [m] safety_check_accel_for_rss: 2.5 # [m/ss] ``` -![fig1](../image/avoidance/safety_check_flowchart.svg) +![fig1](./images/safety_check_flowchart.svg) `safety_check_backward_distance` is the parameter related to the safety check area. The module checks a collision risk for all vehicle that is within shift side lane and between object `object_check_forward_distance` ahead and `safety_check_backward_distance` behind. -![fig1](../image/avoidance/safety_check_step_1.svg) +![fig1](./images/safety_check_step_1.svg) **NOTE**: Even if a part of an object polygon overlaps the detection area, if the center of gravity of the object does not exist on the lane, the vehicle is excluded from the safety check target. @@ -659,11 +659,11 @@ safety_check_accel_for_rss: 2.5 # [m/ss] Judge the risk of collision based on ego future position and object prediction path. The module calculates Ego's future position in the time horizon (`safety_check_time_horizon`), and use object's prediction path as object future position. -![fig1](../image/avoidance/safety_check_step_2.svg) +![fig1](./images/safety_check_step_2.svg) After calculating the future position of Ego and object, the module calculates the lateral/longitudinal deviation of Ego and the object. The module also calculates the lateral/longitudinal margin necessary to determine that it is safe to execute avoidance maneuver, and if both the lateral and longitudinal distances are less than the margins, it determines that there is a risk of a collision at that time. -![fig1](../image/avoidance/safety_check_margin.svg) +![fig1](./images/safety_check_margin.svg) The value of the longitudinal margin is calculated based on Responsibility-Sensitive Safety theory ([RSS](https://newsroom.intel.com/articles/rss-explained-five-rules-autonomous-vehicle-safety/#gs.ljzofv)). The `safety_check_idling_time` represents $T_{idle}$, and `safety_check_accel_for_rss` represents $a_{max}$. @@ -673,7 +673,7 @@ $$ The lateral margin is changeable based on ego longitudinal velocity. If the vehicle is driving at a high speed, the lateral margin should be larger, and if the vehicle is driving at a low speed, the value of the lateral margin should be set to a smaller value. Thus, the lateral margin for each vehicle speed is set as a parameter, and the module determines the lateral margin from the current vehicle speed as shown in the following figure. -![fig1](../image/avoidance/dynamic_lateral_margin.svg) +![fig1](./images/dynamic_lateral_margin.svg) ```yaml target_velocity_matrix: @@ -695,7 +695,7 @@ If an avoidance path can be generated and it is determined that avoidance maneuv yield_velocity: 2.78 # [m/s] ``` -![fig1](../image/avoidance/yield_maneuver.svg) +![fig1](./images/yield_maneuver.svg) **NOTE**: In yield maneuver, the vehicle decelerates target velocity under constraints. @@ -736,9 +736,9 @@ $$ SHIFT_{current} > L_{threshold} $$ -![fig1](../image/avoidance/yield_limitation.svg) +![fig1](./images/yield_limitation.svg) -![fig1](../image/avoidance/yield_maneuver_flowchart.svg) +![fig1](./images/yield_maneuver_flowchart.svg) --- @@ -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 | @@ -935,7 +936,7 @@ namespace: `avoidance.constraints.longitudinal.` - **Planning on the intersection** - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop.　 This is especially important at intersections. -![fig1](../image/avoidance/intersection_problem.drawio.svg) +![fig1](./images/intersection_problem.drawio.svg) - **Safety Check** @@ -963,7 +964,7 @@ namespace: `avoidance.constraints.longitudinal.` Developers can see what is going on in each process by visualizing all the avoidance planning process outputs. The example includes target vehicles, shift points for each object, shift points after each filtering process, etc. -![fig1](../image/avoidance/avoidance-debug-marker.png) +![fig1](./images/avoidance-debug-marker.png) To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. @@ -971,7 +972,7 @@ To enable the debug marker, execute `ros2 param set /planning/scenario_planning/ If for some reason, no shift point is generated for your object, you can check for the failure reason via `ros2 topic echo`. -![avoidance_debug_message_array](../image/avoidance/avoidance_debug_message_array.png) +![avoidance_debug_message_array](./images/avoidance_debug_message_array.png) To print the debug message, just run the following diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index c68c10f2a9489..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 # [-] @@ -141,6 +142,7 @@ force_avoidance: enable: false # [-] time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.0 # [m] diff --git a/planning/behavior_path_planner/image/avoidance/avoidance-debug-marker.png b/planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance-debug-marker.png rename to planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png diff --git a/planning/behavior_path_planner/image/avoidance/avoidance_debug_message_array.png b/planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance_debug_message_array.png rename to planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png diff --git a/planning/behavior_path_planner/image/avoidance/avoidance_design.fig1.drawio.svg b/planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance_design.fig1.drawio.svg rename to planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/avoidance_module-shift_point_and_its_constraints.drawio.png b/planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/avoidance_module-shift_point_and_its_constraints.drawio.png rename to planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png diff --git a/planning/behavior_path_planner/image/avoidance/dynamic_lateral_margin.svg b/planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/dynamic_lateral_margin.svg rename to planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg diff --git a/planning/behavior_path_planner/image/avoidance/envelope_polygon.svg b/planning/behavior_path_avoidance_module/images/envelope_polygon.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/envelope_polygon.svg rename to planning/behavior_path_avoidance_module/images/envelope_polygon.svg diff --git a/planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_both_direction.drawio.svg b/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_both_direction.drawio.svg rename to planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_one_direction.drawio.svg b/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_multi_object_one_direction.drawio.svg rename to planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_one_object.drawio.svg b/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/how_to_decide_path_shape_one_object.drawio.svg rename to planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/intersection_problem.drawio.svg b/planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/intersection_problem.drawio.svg rename to planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/obstacle_to_road_shoulder_distance.drawio.svg b/planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/obstacle_to_road_shoulder_distance.drawio.svg rename to planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/parked-car-detection.svg b/planning/behavior_path_avoidance_module/images/parked-car-detection.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/parked-car-detection.svg rename to planning/behavior_path_avoidance_module/images/parked-car-detection.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_flowchart.svg b/planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_flowchart.svg rename to planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_margin.svg b/planning/behavior_path_avoidance_module/images/safety_check_margin.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_margin.svg rename to planning/behavior_path_avoidance_module/images/safety_check_margin.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_step_1.svg b/planning/behavior_path_avoidance_module/images/safety_check_step_1.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_step_1.svg rename to planning/behavior_path_avoidance_module/images/safety_check_step_1.svg diff --git a/planning/behavior_path_planner/image/avoidance/safety_check_step_2.svg b/planning/behavior_path_avoidance_module/images/safety_check_step_2.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/safety_check_step_2.svg rename to planning/behavior_path_avoidance_module/images/safety_check_step_2.svg diff --git a/planning/behavior_path_planner/image/shift_length_parameters.drawio.svg b/planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/shift_length_parameters.drawio.svg rename to planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg diff --git a/planning/behavior_path_planner/image/avoidance/shift_line_generation.svg b/planning/behavior_path_avoidance_module/images/shift_line_generation.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/shift_line_generation.svg rename to planning/behavior_path_avoidance_module/images/shift_line_generation.svg diff --git a/planning/behavior_path_planner/image/avoidance/target_vehicle_selection.svg b/planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/target_vehicle_selection.svg rename to planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg diff --git a/planning/behavior_path_planner/image/avoidance/todo.png b/planning/behavior_path_avoidance_module/images/todo.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance/todo.png rename to planning/behavior_path_avoidance_module/images/todo.png diff --git a/planning/behavior_path_planner/image/avoidance/use_adjacent_lane.svg b/planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_adjacent_lane.svg rename to planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg diff --git a/planning/behavior_path_planner/image/avoidance/use_hatched_road_markings.svg b/planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_hatched_road_markings.svg rename to planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg diff --git a/planning/behavior_path_planner/image/avoidance/use_intersection_areas.svg b/planning/behavior_path_avoidance_module/images/use_intersection_areas.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_intersection_areas.svg rename to planning/behavior_path_avoidance_module/images/use_intersection_areas.svg diff --git a/planning/behavior_path_planner/image/avoidance/use_opposite_lane.svg b/planning/behavior_path_avoidance_module/images/use_opposite_lane.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/use_opposite_lane.svg rename to planning/behavior_path_avoidance_module/images/use_opposite_lane.svg diff --git a/planning/behavior_path_planner/image/avoidance/yield_limitation.svg b/planning/behavior_path_avoidance_module/images/yield_limitation.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/yield_limitation.svg rename to planning/behavior_path_avoidance_module/images/yield_limitation.svg diff --git a/planning/behavior_path_planner/image/avoidance/yield_maneuver.svg b/planning/behavior_path_avoidance_module/images/yield_maneuver.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/yield_maneuver.svg rename to planning/behavior_path_avoidance_module/images/yield_maneuver.svg diff --git a/planning/behavior_path_planner/image/avoidance/yield_maneuver_flowchart.svg b/planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg similarity index 100% rename from planning/behavior_path_planner/image/avoidance/yield_maneuver_flowchart.svg rename to planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg 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 8c45230959c6c..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 @@ -19,12 +19,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 +#include #include #include #include -#include #include #include @@ -34,6 +33,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -48,10 +48,11 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::TransformStamped; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using route_handler::Direction; + struct ObjectParameter { bool is_avoidance_target{false}; @@ -163,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}; @@ -175,6 +180,7 @@ struct AvoidanceParameters // force avoidance double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; + double force_avoidance_distance_threshold{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction @@ -326,24 +332,38 @@ struct AvoidanceParameters struct ObjectData // avoidance target { ObjectData() = default; - ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang) - : object(obj), lateral(lat), longitudinal(lon), length(len), overhang_dist(overhang) + + ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang) + : object(std::move(obj)), + to_centerline(lat), + longitudinal(lon), + length(len), + overhang_dist(overhang) { } PredictedObject object; + // object behavior. + enum class Behavior { + NONE = 0, + MERGING, + DEVIATING, + }; + Behavior behavior{Behavior::NONE}; + // lateral position of the CoM, in Frenet coordinate from ego-pose - double lateral; + + double to_centerline{0.0}; // longitudinal position of the CoM, in Frenet coordinate from ego-pose - double longitudinal; + double longitudinal{0.0}; // longitudinal length of vehicle, in Frenet coordinate - double length; + double length{0.0}; // lateral distance to the closest footprint, in Frenet coordinate - double overhang_dist; + double overhang_dist{0.0}; // lateral shiftable ratio double shiftable_ratio{0.0}; @@ -366,6 +386,9 @@ struct ObjectData // avoidance target // store the information of the lanelet which the object's overhang is currently occupying lanelet::ConstLanelet overhang_lanelet; + // the position at the detected moment + Pose init_pose; + // the position of the overhang Pose overhang_pose; @@ -396,11 +419,17 @@ struct ObjectData // avoidance target // is within intersection area bool is_within_intersection{false}; + // object direction. + Direction direction{Direction::NONE}; + // unavoidable reason - std::string reason{""}; + std::string reason{}; // lateral avoid margin std::optional avoid_margin{std::nullopt}; + + // the nearest bound point (use in road shoulder distance calculation) + std::optional nearest_bound_point{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -437,14 +466,14 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(const AvoidLine & avoid_line, const AvoidLine & return_line) - : avoid_line{avoid_line}, return_line{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{}; }; @@ -517,6 +546,8 @@ struct AvoidancePlanningData bool valid{false}; + bool success{false}; + bool comfortable{false}; bool avoid_required{false}; @@ -561,9 +592,7 @@ struct ShiftLineData */ struct DebugData { - std::shared_ptr current_lanelets; - - geometry_msgs::msg::Polygon detection_area; + std::vector detection_areas; lanelet::ConstLineStrings3d bounds; @@ -613,6 +642,9 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // safety check area + lanelet::ConstLanelets safety_check_lanes; + // collision check debug map utils::path_safety_checker::CollisionCheckDebugMap collision_check; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 2bf3158da9ab6..f49f457ddd066 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -16,33 +16,23 @@ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ #include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" #include -#include #include -#include #include -#include - #include -#include namespace marker_utils::avoidance_marker { -using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidancePlanningData; -using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::PathShifter; using behavior_path_planner::ShiftLineArray; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using visualization_msgs::msg::MarkerArray; @@ -50,7 +40,7 @@ MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); MarkerArray createAvoidLineMarkerArray( - const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, + const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w); MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); @@ -59,22 +49,15 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray makeOverhangToRoadShoulderMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns); - -MarkerArray createOverhangFurthestLineStringMarkerArray( - const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r, - const float & g, const float & b); - MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); } // namespace marker_utils::avoidance_marker -std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sp_arr); +std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr); -std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); +std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 6717cefefc9f6..f5b797978b744 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -58,7 +58,9 @@ class AvoidanceHelper double getEgoSpeed() const { return std::abs(data_->self_odometry->twist.twist.linear.x); } - size_t getConstraintsMapIndex(const double velocity, const std::vector & values) const + geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } + + static size_t getConstraintsMapIndex(const double velocity, const std::vector & values) { const auto itr = std::find_if( values.begin(), values.end(), [&](const auto value) { return velocity < value; }); @@ -185,10 +187,20 @@ class AvoidanceHelper return parameters_->object_check_max_forward_distance; } + const auto & route_handler = data_->route_handler; + + lanelet::ConstLanelet closest_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &closest_lane)) { + return parameters_->object_check_max_forward_distance; + } + + const auto limit = route_handler->getTrafficRulesPtr()->speedLimit(closest_lane); + const auto speed = isShifted() ? limit.speedLimit.value() : getEgoSpeed(); + const auto max_shift_length = std::max( std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); - const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk( - max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); + const auto dynamic_distance = + PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed); return std::clamp( 1.5 * dynamic_distance + getNominalPrepareDistance(), diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 72cfbe6f0a1ed..e23a8a96ee7da 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -19,11 +19,10 @@ #include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include +#include +#include #include -#include -#include #include namespace behavior_path_planner 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 new file mode 100644 index 0000000000000..9f2fdf7ab96d9 --- /dev/null +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -0,0 +1,370 @@ +// 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_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ + +#include "tier4_autoware_utils/ros/parameter.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using tier4_autoware_utils::getOrDeclareParameter; + +AvoidanceParameters getParameter(rclcpp::Node * node) +{ + AvoidanceParameters p{}; + // general params + { + const std::string ns = "avoidance."; + p.resample_interval_for_planning = + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); + p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); + p.enable_yield_maneuver_during_shifting = + getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); + p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); + p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); + p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); + } + + // drivable area + { + const std::string ns = "avoidance."; + p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); + p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); + p.use_hatched_road_markings = + getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.safety_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); + param.use_conservative_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); + return param; + }; + + const std::string ns = "avoidance.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + 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 = + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_check_yaw_deviation = + getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + const std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.force_avoidance_distance_threshold = + getOrDeclareParameter(*node, ns + "distance_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + + // safety check general params + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_safety_check_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.safety_check."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); + p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); + p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); + p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); + p.check_unavoidable_object = + getOrDeclareParameter(*node, ns + "check_unavoidable_object"); + p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); + p.check_all_predicted_path = + getOrDeclareParameter(*node, ns + "check_all_predicted_path"); + p.safety_check_backward_distance = + getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = + getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + } + + // safety check predicted path params + { + const std::string ns = "avoidance.safety_check."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(*node, ns + "min_velocity"); + p.ego_predicted_path_params.max_velocity = + getOrDeclareParameter(*node, ns + "max_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_front_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(*node, ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(*node, ns + "delay_until_departure"); + } + + // safety check rss params + { + const std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + getOrDeclareParameter(*node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); + } + + // avoidance maneuver (lateral) + { + const std::string ns = "avoidance.avoidance.lateral."; + p.soft_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); + p.hard_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); + p.lateral_execution_threshold = + getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); + p.lateral_small_shift_threshold = + getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); + p.lateral_avoid_check_threshold = + getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); + p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); + p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + p.max_deviation_from_lane = + getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); + } + + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + } + + // avoidance maneuver (return shift dead line) + { + const std::string ns = "avoidance.avoidance.return_dead_line."; + p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + p.dead_line_buffer_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + } + + // yield + { + const std::string ns = "avoidance.yield."; + p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); + } + + // stop + { + const std::string ns = "avoidance.stop."; + p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); + p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); + } + + // policy + { + const std::string ns = "avoidance.policy."; + p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); + p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); + p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); + p.use_shorten_margin_immediately = + getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); + + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + + if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + } + + // constraints (longitudinal) + { + const std::string ns = "avoidance.constraints.longitudinal."; + p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); + p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); + p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); + p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); + p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + } + + // constraints (lateral) + { + const std::string ns = "avoidance.constraints.lateral."; + p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); + p.lateral_max_accel_map = + getOrDeclareParameter>(*node, ns + "max_accel_values"); + p.lateral_min_jerk_map = + getOrDeclareParameter>(*node, ns + "min_jerk_values"); + p.lateral_max_jerk_map = + getOrDeclareParameter>(*node, ns + "max_jerk_values"); + + if (p.velocity_map.empty()) { + throw std::domain_error("invalid velocity map."); + } + + if (p.velocity_map.size() != p.lateral_max_accel_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + } + + // shift line pipeline + { + const std::string ns = "avoidance.shift_line_pipeline."; + p.quantize_filter_threshold = + getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); + p.same_grad_filter_1_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); + p.same_grad_filter_2_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); + p.same_grad_filter_3_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); + p.sharp_shift_filter_threshold = + getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); + } + return p; +} +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ 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 6a66b485d1c59..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 @@ -20,9 +20,10 @@ #include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include +#include +#include +#include #include #include @@ -30,11 +31,9 @@ #include #include -#include #include #include #include -#include #include namespace behavior_path_planner @@ -73,6 +72,8 @@ class AvoidanceModule : public SceneModuleInterface std::shared_ptr get_debug_msg_array() const; private: + bool isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const; + bool canTransitSuccessState() override; bool canTransitFailureState() override { return false; } @@ -111,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) { @@ -129,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) { @@ -324,13 +323,6 @@ class AvoidanceModule : public SceneModuleInterface // generate output data - /** - * @brief calculate turn signal infomation. - * @param avoidance path. - * @return turn signal command. - */ - TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - /** * @brief fill debug markers. */ @@ -372,8 +364,8 @@ class AvoidanceModule : public SceneModuleInterface */ void removeRegisteredShiftLines() { - constexpr double THRESHOLD = 0.1; - if (std::abs(path_shifter_.getBaseOffset()) > THRESHOLD) { + constexpr double threshold = 0.1; + if (std::abs(path_shifter_.getBaseOffset()) > threshold) { RCLCPP_INFO(getLogger(), "base offset is not zero. can't reset registered shift lines."); return; } @@ -394,7 +386,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief remove behind shift lines. * @param path shifter. */ - void postProcess() + void postProcess() override { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); @@ -405,12 +397,11 @@ class AvoidanceModule : public SceneModuleInterface UUID uuid; Pose start_pose; Pose finish_pose; + double relative_longitudinal{0.0}; }; using RegisteredShiftLineArray = std::vector; - bool is_avoidance_maneuver_starts; - bool arrived_path_end_{false}; bool safe_{true}; @@ -431,14 +422,20 @@ class AvoidanceModule : public SceneModuleInterface UUID candidate_uuid_; + // TODO(Satoshi OTA) create detected object manager. ObjectDataArray registered_objects_; - mutable size_t safe_count_{0}; + // TODO(Satoshi OTA) remove mutable. + mutable ObjectDataArray detected_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray ego_stopped_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray stopped_objects_; + mutable size_t safe_count_{0}; + mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index edfe9ab9663ce..efdda622a664c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -17,11 +17,8 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include - #include namespace behavior_path_planner::utils::avoidance @@ -29,8 +26,6 @@ namespace behavior_path_planner::utils::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::helper::avoidance::AvoidanceHelper; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; class ShiftLineGenerator { 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 c954ab9022853..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); @@ -117,6 +112,8 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects); + void updateRegisteredObject( ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, const std::shared_ptr & parameters); @@ -126,22 +123,10 @@ 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); -double extendToRoadShoulderDistanceWithPolygon( - const std::shared_ptr & rh, - const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, - const bool use_intersection_areas); - void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); @@ -158,16 +143,18 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift); + const std::shared_ptr & parameters, const bool is_right_shift, + 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, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters, const bool in_avoidance_maneuver); double calcDistanceToReturnDeadLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, @@ -178,6 +165,10 @@ double calcDistanceToAvoidStartLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); + +TurnSignalInfo calcTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, + const AvoidancePlanningData & data, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml index 642f83497acfb..f72adf0591493 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/behavior_path_avoidance_module/package.xml @@ -27,7 +27,6 @@ behavior_path_planner_common geometry_msgs lanelet2_extension - libboost-dev magic_enum motion_utils objects_of_interest_marker_interface diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 3ae210e0b1d78..f088b9228d964 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -14,16 +14,15 @@ #include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include -#include -#include - #include #include @@ -109,6 +108,42 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std: return msg; } +MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + for (const auto & object : objects) { + if (!object.nearest_bound_point.has_value()) { + continue; + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999)); + + marker.points.push_back(object.overhang_pose.position); + marker.points.push_back(object.nearest_bound_point.value()); + marker.id = uuidToInt32(object.object.object_id); + msg.markers.push_back(marker); + } + + { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + marker.pose.position = object.nearest_bound_point.value(); + std::ostringstream string_stream; + string_stream << object.to_road_shoulder_distance << "[m]"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + } + + return msg; +} + MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; @@ -124,7 +159,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral:" << object.lateral << " [m]\n" + << "lateral:" << object.to_centerline << " [m]\n" << "necessity:" << object.avoid_required << " [-]\n" << "stoppable:" << object.is_stoppable << " [-]\n" << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" @@ -166,6 +201,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); return msg; } @@ -183,6 +219,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); return msg; } @@ -418,58 +455,37 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const return msg; } -MarkerArray makeOverhangToRoadShoulderMarkerArray( - const ObjectDataArray & objects, std::string && ns) -{ - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 1.0)); - - int32_t i{0}; - MarkerArray msg; - for (const auto & object : objects) { - marker.id = ++i; - marker.pose = object.overhang_pose; - std::ostringstream string_stream; - string_stream << "(to_road_shoulder_distance = " << object.to_road_shoulder_distance << " [m])"; - marker.text = string_stream.str(); - msg.markers.push_back(marker); - } - - return msg; -} - -MarkerArray createOverhangFurthestLineStringMarkerArray( - const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r, - const float & g, const float & b) +MarkerArray createDrivableBounds( + const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g, + const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); MarkerArray msg; - for (const auto & linestring : linestrings) { - const auto id = static_cast(linestring.id()); + // right bound + { auto marker = createDefaultMarker( - "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), + "map", current_time, ns + "_right", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - for (const auto & p : linestring.basicLineString()) { + for (const auto & p : data.right_bound) { marker.points.push_back(createPoint(p.x(), p.y(), p.z())); } + msg.markers.push_back(marker); + } + + // left bound + { + auto marker = createDefaultMarker( + "map", current_time, ns + "_left", 0L, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), + createMarkerColor(r, g, b, 0.999)); - Marker marker_linestring_id = createDefaultMarker( - "map", current_time, "linestring_id", id, Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.8)); - Pose text_id_pose; - text_id_pose.position.x = linestring.front().x(); - text_id_pose.position.y = linestring.front().y(); - text_id_pose.position.z = linestring.front().z(); - marker_linestring_id.pose = text_id_pose; - std::ostringstream ss; - ss << "(ID : " << id << ") "; - marker_linestring_id.text = ss.str(); - msg.markers.push_back(marker_linestring_id); + for (const auto & p : data.left_bound) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + + msg.markers.push_back(marker); } return msg; @@ -478,6 +494,8 @@ MarkerArray createOverhangFurthestLineStringMarkerArray( MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) { + using behavior_path_planner::utils::transformToLanelets; + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; @@ -537,6 +555,8 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); addObjects(data.other_objects, std::string("TooNearToGoal")); + addObjects(data.other_objects, std::string("ParallelToEgoLane")); + addObjects(data.other_objects, std::string("MergingToEgoLane")); } // shift line pre-process @@ -588,13 +608,23 @@ 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(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); + add(laneletsAsTriangleMarkerArray( + "drivable_lanes", transformToLanelets(data.drivable_lanes), + createMarkerColor(0.16, 1.0, 0.69, 0.2))); + add(laneletsAsTriangleMarkerArray( + "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2))); + add(laneletsAsTriangleMarkerArray( + "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); } return msg; diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index f4997d2af12f1..68cee1aa2b523 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "behavior_path_avoidance_module/manager.hpp" +#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -33,332 +34,7 @@ void AvoidanceModuleManager::init(rclcpp::Node * node) // init manager interface initInterface(node, {"left", "right"}); - AvoidanceParameters p{}; - // general params - { - const std::string ns = "avoidance."; - p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); - p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); - p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); - p.enable_yield_maneuver_during_shifting = - getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); - p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); - p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); - } - - // drivable area - { - const std::string ns = "avoidance."; - p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); - p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); - p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); - p.use_hatched_road_markings = - getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); - } - - // target object - { - const auto get_object_param = [&](std::string && ns) { - ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); - param.safety_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); - param.use_conservative_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); - return param; - }; - - const std::string ns = "avoidance.target_object."; - p.object_parameters.emplace( - ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); - p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); - p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); - p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); - p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); - p.object_parameters.emplace( - ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); - p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); - p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); - - p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); - p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); - } - - // target filtering - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.target_filtering."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_check_yaw_deviation = - getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); - } - - { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); - } - - { - const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); - p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); - p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); - } - - // safety check general params - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_safety_check_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.safety_check."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); - p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); - p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); - p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); - p.check_unavoidable_object = - getOrDeclareParameter(*node, ns + "check_unavoidable_object"); - p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); - p.check_all_predicted_path = - getOrDeclareParameter(*node, ns + "check_all_predicted_path"); - p.safety_check_backward_distance = - getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); - p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); - p.hysteresis_factor_safe_count = - getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); - } - - // safety check predicted path params - { - const std::string ns = "avoidance.safety_check."; - p.ego_predicted_path_params.min_velocity = - getOrDeclareParameter(*node, ns + "min_velocity"); - p.ego_predicted_path_params.max_velocity = - getOrDeclareParameter(*node, ns + "max_velocity"); - p.ego_predicted_path_params.acceleration = - getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); - p.ego_predicted_path_params.time_horizon_for_rear_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); - p.ego_predicted_path_params.time_resolution = - getOrDeclareParameter(*node, ns + "time_resolution"); - p.ego_predicted_path_params.delay_until_departure = - getOrDeclareParameter(*node, ns + "delay_until_departure"); - } - - // safety check rss params - { - const std::string ns = "avoidance.safety_check."; - p.rss_params.longitudinal_distance_min_threshold = - getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); - p.rss_params.longitudinal_velocity_delta_time = - getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_front_deceleration"); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); - } - - // avoidance maneuver (lateral) - { - const std::string ns = "avoidance.avoidance.lateral."; - p.soft_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); - p.hard_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); - p.lateral_execution_threshold = - getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); - p.lateral_small_shift_threshold = - getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); - p.lateral_avoid_check_threshold = - getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); - p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); - p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); - p.max_deviation_from_lane = - getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); - } - - // avoidance maneuver (longitudinal) - { - const std::string ns = "avoidance.avoidance.longitudinal."; - p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); - p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); - p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); - p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); - p.nominal_avoidance_speed = - getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); - } - - // avoidance maneuver (return shift dead line) - { - const std::string ns = "avoidance.avoidance.return_dead_line."; - p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); - p.enable_dead_line_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.enable"); - p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); - p.dead_line_buffer_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.buffer"); - } - - // yield - { - const std::string ns = "avoidance.yield."; - p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); - } - - // stop - { - const std::string ns = "avoidance.stop."; - p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); - p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); - } - - // policy - { - const std::string ns = "avoidance.policy."; - p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); - p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); - p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); - p.use_shorten_margin_immediately = - getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); - - if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { - throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); - } - - if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { - throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); - } - } - - // constraints (longitudinal) - { - const std::string ns = "avoidance.constraints.longitudinal."; - p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); - p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); - p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); - p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); - p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); - } - - // constraints (lateral) - { - const std::string ns = "avoidance.constraints.lateral."; - p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); - p.lateral_max_accel_map = - getOrDeclareParameter>(*node, ns + "max_accel_values"); - p.lateral_min_jerk_map = - getOrDeclareParameter>(*node, ns + "min_jerk_values"); - p.lateral_max_jerk_map = - getOrDeclareParameter>(*node, ns + "max_jerk_values"); - - if (p.velocity_map.empty()) { - throw std::domain_error("invalid velocity map."); - } - - if (p.velocity_map.size() != p.lateral_max_accel_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - - if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - - if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - } - - // shift line pipeline - { - const std::string ns = "avoidance.shift_line_pipeline."; - p.quantize_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); - p.same_grad_filter_1_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); - p.same_grad_filter_2_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); - p.same_grad_filter_3_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); - p.sharp_shift_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); - } + auto p = getParameter(node); parameters_ = std::make_shared(p); } diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 9b0c85f60ba29..bc1d8c6d75424 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -17,9 +17,9 @@ #include "behavior_path_avoidance_module/debug.hpp" #include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.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/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -28,14 +28,11 @@ #include #include #include -#include -#include -#include -#include +#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include #include -#include #include #include @@ -137,7 +134,48 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoid_data_.safe && avoid_data_.comfortable; + return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid; +} + +bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const +{ + const bool has_avoidance_target = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); + + if (has_avoidance_target) { + return false; + } + + // If the ego is on the shift line, keep RUNNING. + { + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + const auto within = [](const auto & line, const size_t idx) { + return line.start_idx < idx && idx < line.end_idx; + }; + for (const auto & shift_line : path_shifter_.getShiftLines()) { + if (within(shift_line, idx)) { + return false; + } + } + } + + const bool has_shift_point = !path_shifter_.getShiftLines().empty(); + const bool has_base_offset = + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + + // Nothing to do. -> EXIT. + if (!has_shift_point && !has_base_offset) { + return true; + } + + // Be able to canceling avoidance path. -> EXIT. + if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) { + return true; + } + + return false; } bool AvoidanceModule::canTransitSuccessState() @@ -168,44 +206,7 @@ bool AvoidanceModule::canTransitSuccessState() } } - const bool has_avoidance_target = - std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { - return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; - }); - const bool has_shift_point = !path_shifter_.getShiftLines().empty(); - const bool has_base_offset = - std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; - - // If the vehicle is on the shift line, keep RUNNING. - { - const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto within = [](const auto & line, const size_t idx) { - return line.start_idx < idx && idx < line.end_idx; - }; - for (const auto & shift_line : path_shifter_.getShiftLines()) { - if (within(shift_line, idx)) { - return false; - } - } - } - - // Nothing to do. -> EXIT. - if (!has_avoidance_target) { - if (!has_shift_point && !has_base_offset) { - RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit."); - return true; - } - } - - // Be able to canceling avoidance path. -> EXIT. - if (!has_avoidance_target) { - if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) { - RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit."); - return true; - } - } - - return false; // Keep current state. + return data.success; } void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) @@ -216,21 +217,23 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // lanelet info data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_); + getPreviousModuleOutput().reference_path, planner_data_); data.extend_lanelets = utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes + const auto has_shift_point = !path_shifter_.getShiftLines().empty(); + const auto in_avoidance_maneuver = has_shift_point || helper_->isShifted(); std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back( - utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + lanelet, planner_data_, parameters_, in_avoidance_maneuver)); }); // calc drivable bound - const auto shorten_lanes = - utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes); + auto tmp_path = getPreviousModuleOutput().path; + const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, true)); @@ -240,9 +243,9 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { - data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + data.reference_path_rough = extendBackwardLength(getPreviousModuleOutput().path); } else { - data.reference_path_rough = *getPreviousModuleOutput().path; + data.reference_path_rough = getPreviousModuleOutput().path; RCLCPP_WARN(getLogger(), "Previous module lane is updated. Don't use latest reference path."); } @@ -284,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; @@ -313,25 +313,25 @@ 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_); }); // debug { - debug.current_lanelets = std::make_shared(data.current_lanelets); - std::vector debug_info_array; const auto append = [&](const auto & o) { AvoidanceDebugMsg debug_info; debug_info.object_id = toHexString(o.object.object_id); debug_info.longitudinal_distance = o.longitudinal; - debug_info.lateral_distance_from_centerline = o.lateral; + debug_info.lateral_distance_from_centerline = o.to_centerline; debug_info.allow_avoidance = o.reason == ""; debug_info.failed_reason = o.reason; debug_info_array.push_back(debug_info); @@ -375,17 +375,13 @@ ObjectData AvoidanceModule::createObjectData( // Calc moving time. utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); + // Fill init pose. + utils::avoidance::fillInitialPose(object_data, detected_objects_); - // 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_); + // Calc lateral deviation from path to target object. + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; return object_data; } @@ -450,15 +446,14 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * STEP1: Create candidate shift lines. * Merge rough shift lines, and extract new shift lines. */ - const auto processed_shift_lines = generator_.generate(data, debug); + data.new_shift_line = generator_.generate(data, debug); /** * Step2: Validate new shift lines. * Output new shift lines only when the avoidance path which is generated from them doesn't have * huge offset from ego. */ - data.valid = isValidShiftLine(processed_shift_lines, path_shifter); - data.new_shift_line = data.valid ? processed_shift_lines : AvoidLineArray{}; + data.valid = isValidShiftLine(data.new_shift_line, path_shifter); const auto found_new_sl = data.new_shift_line.size() > 0; const auto registered = path_shifter.getShiftLines().size() > 0; data.found_avoidance_path = found_new_sl || registered; @@ -494,16 +489,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - /** - * TODO(someone): prevent meaningless stop point insertion in other way. - * If the candidate shift line is invalid, manage all objects as unavoidable. - */ - if (!data.valid) { - std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) { - o.is_avoidable = false; - o.reason = "InvalidShiftLine"; - }); - } + data.success = isSatisfiedSuccessCondition(data); /** * Find the nearest object that should be avoid. When the ego follows reference path, @@ -705,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,7 +715,23 @@ bool AvoidanceModule::isSafePath( const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, is_right_shift.value()); + 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); @@ -795,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())); + auto lines = path_shifter_.getShiftLines(); + if (lines.empty()) { + return max_dist; } - for (const auto & sp : generator_.getRawRegisteredShiftLine()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); - } - 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. @@ -857,6 +848,10 @@ BehaviorModuleOutput AvoidanceModule::plan() updatePathShifter(data.safe_shift_line); + if (data.success) { + removeRegisteredShiftLines(); + } + if (data.yield_required) { removeRegisteredShiftLines(); } @@ -873,7 +868,7 @@ BehaviorModuleOutput AvoidanceModule::plan() if (success_spline_path_generation && success_linear_path_generation) { helper_->setPreviousLinearShiftPath(linear_shift_path); helper_->setPreviousSplineShiftPath(spline_shift_path); - helper_->setPreviousReferencePath(data.reference_path); + helper_->setPreviousReferencePath(path_shifter_.getReferencePath()); } else { spline_shift_path = helper_->getPreviousSplineShiftPath(); } @@ -886,9 +881,13 @@ BehaviorModuleOutput AvoidanceModule::plan() BehaviorModuleOutput output; // turn signal info - { + if (path_shifter_.getShiftLines().empty()) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = calcTurnSignalInfo(spline_shift_path); + const auto new_signal = utils::avoidance::calcTurnSignalInfo( + linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, + planner_data_); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -912,17 +911,17 @@ BehaviorModuleOutput AvoidanceModule::plan() } if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { - output.path = std::make_shared(spline_shift_path.path); + output.path = spline_shift_path.path; } else { output.path = getPreviousModuleOutput().path; RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing."); } output.reference_path = getPreviousModuleOutput().reference_path; - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - const size_t ego_idx = planner_data_->findEgoIndex(output.path->points); - utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); + const size_t ego_idx = planner_data_->findEgoIndex(output.path.points); + utils::clipPathLength(output.path, ego_idx, planner_data_->parameters); // Drivable area generation. { @@ -997,7 +996,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() } path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return out; } @@ -1023,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(); @@ -1144,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."); @@ -1171,11 +1177,11 @@ void AvoidanceModule::updateData() helper_->setData(planner_data_); if (!helper_->isInitialized()) { - helper_->setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_->setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_->setPreviousReferencePath(*getPreviousModuleOutput().path); + helper_->setPreviousSplineShiftPath(toShiftedPath(getPreviousModuleOutput().path)); + helper_->setPreviousLinearShiftPath(toShiftedPath(getPreviousModuleOutput().path)); + helper_->setPreviousReferencePath(getPreviousModuleOutput().path); helper_->setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_)); + getPreviousModuleOutput().reference_path, planner_data_)); } debug_data_ = DebugData(); @@ -1207,6 +1213,12 @@ void AvoidanceModule::updateData() // update rtc status. updateRTCData(); + // update interest objects data + 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); + } + safe_ = avoid_data_.safe; } @@ -1231,7 +1243,6 @@ void AvoidanceModule::initVariables() debug_marker_.markers.clear(); resetPathCandidate(); resetPathReference(); - is_avoidance_maneuver_starts = false; arrived_path_end_ = false; } @@ -1268,8 +1279,8 @@ void AvoidanceModule::updateRTCData() CandidateOutput output; - const auto sl_front = candidates.front(); - const auto sl_back = candidates.back(); + const auto & sl_front = candidates.front(); + const auto & sl_back = candidates.back(); output.path_candidate = data.candidate_path.path; output.lateral_shift = helper_->getRelativeShiftToPath(shift_line); @@ -1279,118 +1290,6 @@ void AvoidanceModule::updateRTCData() updateCandidateRTCStatus(output); } -TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const -{ - const auto shift_lines = path_shifter_.getShiftLines(); - if (shift_lines.empty()) { - return {}; - } - - const auto front_shift_line = shift_lines.front(); - const size_t start_idx = front_shift_line.start_idx; - const size_t end_idx = front_shift_line.end_idx; - - const auto current_shift_length = helper_->getEgoShift(); - const double start_shift_length = path.shift_length.at(start_idx); - const double end_shift_length = path.shift_length.at(end_idx); - const double segment_shift_length = end_shift_length - start_shift_length; - - const double turn_signal_shift_length_threshold = - planner_data_->parameters.turn_signal_shift_length_threshold; - const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time; - const double turn_signal_minimum_search_distance = - planner_data_->parameters.turn_signal_minimum_search_distance; - - // If shift length is shorter than the threshold, it does not need to turn on blinkers - if (std::fabs(segment_shift_length) < turn_signal_shift_length_threshold) { - return {}; - } - - // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < 0.1) { - return {}; - } - - // compute blinker start idx and end idx - size_t blinker_start_idx = [&]() { - for (size_t idx = start_idx; idx <= end_idx; ++idx) { - const double current_shift_length = path.shift_length.at(idx); - if (current_shift_length > 0.1) { - return idx; - } - } - return start_idx; - }(); - size_t blinker_end_idx = end_idx; - - // prevent invalid access for out-of-range - blinker_start_idx = - std::min(std::max(std::size_t(0), blinker_start_idx), path.path.points.size() - 1); - blinker_end_idx = - std::min(std::max(blinker_start_idx, blinker_end_idx), path.path.points.size() - 1); - - const auto blinker_start_pose = path.path.points.at(blinker_start_idx).point.pose; - const auto blinker_end_pose = path.path.points.at(blinker_end_idx).point.pose; - - const double ego_vehicle_offset = - planner_data_->parameters.vehicle_info.max_longitudinal_offset_m; - const auto signal_prepare_distance = - std::max(getEgoSpeed() * turn_signal_search_time, turn_signal_minimum_search_distance); - const auto ego_front_to_shift_start = - calcSignedArcLength(path.path.points, getEgoPosition(), blinker_start_pose.position) - - ego_vehicle_offset; - - if (signal_prepare_distance < ego_front_to_shift_start) { - return {}; - } - - bool turn_signal_on_swerving = planner_data_->parameters.turn_signal_on_swerving; - - TurnSignalInfo turn_signal_info{}; - if (turn_signal_on_swerving) { - if (segment_shift_length > 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } else { - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); - const auto local_vehicle_footprint = - createVehicleFootprint(planner_data_->parameters.vehicle_info); - boost::geometry::model::ring shifted_vehicle_footprint; - for (const auto & cl : current_lanes) { - // get left and right bounds of current lane - const auto lane_left_bound = cl.leftBound2d().basicLineString(); - const auto lane_right_bound = cl.rightBound2d().basicLineString(); - for (size_t i = start_idx; i < end_idx; ++i) { - // transform vehicle footprint onto path points - shifted_vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose)); - if ( - boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) || - boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) { - if (segment_shift_length > 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } - } - } - } - if (ego_front_to_shift_start > 0.0) { - turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; - } else { - turn_signal_info.desired_start_point = blinker_start_pose; - } - turn_signal_info.desired_end_point = blinker_end_pose; - turn_signal_info.required_start_point = blinker_start_pose; - turn_signal_info.required_end_point = blinker_end_pose; - - return turn_signal_info; -} - void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; @@ -1491,7 +1390,8 @@ void AvoidanceModule::insertReturnDeadLine( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); - const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length); + const auto min_return_distance = + helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point @@ -1702,7 +1602,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); - if (current_target_velocity < getEgoSpeed() && decel_distance < remaining_distance) { + if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { utils::avoidance::insertDecelPoint( getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, slow_pose_); 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 108412876d47f..6196b3e209d11 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -19,7 +19,7 @@ #include -#include +#include namespace behavior_path_planner::utils::avoidance { @@ -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); } @@ -890,7 +929,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { continue; } @@ -931,6 +970,9 @@ void ShiftLineGenerator::applySimilarGradFilter( combine, input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx, input.at(i).end_longitudinal); + combine.parent_ids = + utils::avoidance::concatParentIds(base_line.parent_ids, input.at(i).parent_ids); + combine_buffer.push_back(input.at(i)); const auto violates = [&]() { @@ -970,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; }); @@ -1024,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. @@ -1067,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); @@ -1098,12 +1172,14 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = std::max(nominal_prepare_distance, 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) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); - prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; + prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1117,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); @@ -1131,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; @@ -1216,7 +1292,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { break; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index b469e9070baad..3a9b5db283304 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -21,29 +21,23 @@ #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include -#include -#include #include -#include #include -#include +#include +#include -#include +#include #include #include #include -#include #include -#include #include #include #include #include -#include #include #include @@ -53,6 +47,7 @@ namespace behavior_path_planner::utils::avoidance using autoware_perception_msgs::msg::TrafficSignalElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; +using geometry_msgs::msg::TransformStamped; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -65,7 +60,6 @@ using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; using tier4_planning_msgs::msg::AvoidanceDebugFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsg; @@ -181,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{}; @@ -189,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) { @@ -200,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) { @@ -231,6 +248,98 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } +bool existShiftSideLane( + const double start_shift_length, const double end_shift_length, const bool no_left_lanes, + const bool no_right_lanes) +{ + constexpr double THRESHOLD = 0.1; + const auto relative_shift_length = end_shift_length - start_shift_length; + + const auto avoid_shift = + std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD; + if (avoid_shift) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + } + + const auto return_shift = + std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD; + if (return_shift) { + // Right return. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD; + if (left_middle_shift) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD; + if (right_middle_shift) { + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + } + + return true; +} + +bool straddleRoadBound( + const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { + const auto transform = pose2transform(path.path.points.at(i).point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; +} + } // namespace namespace filtering_utils @@ -333,6 +442,34 @@ bool isParallelToEgoLane(const ObjectData & object, const double threshold) return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } +bool isMergingToEgoLane(const ObjectData & object) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose); + + if (isOnRight(object)) { + if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { + return false; + } + + if (yaw_deviation > M_PI_2) { + return false; + } + } else { + if (yaw_deviation > 0.0 && M_PI_2 > yaw_deviation) { + return false; + } + + if (yaw_deviation < -1.0 * M_PI_2) { + return false; + } + } + + return true; +} + bool isObjectOnRoadShoulder( ObjectData & object, const std::shared_ptr & route_handler, const std::shared_ptr & parameters) @@ -463,6 +600,15 @@ bool isForceAvoidanceTarget( return false; } + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > + parameters->force_avoidance_distance_threshold; + + if (is_moving_distance_longer_than_threshold) { + return false; + } + if (object.is_within_intersection) { RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); return false; @@ -478,7 +624,6 @@ bool isForceAvoidanceTarget( } const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle bool not_parked_object = true; @@ -507,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) { @@ -534,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; } @@ -555,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; @@ -579,6 +726,17 @@ bool isSatisfiedWithNonVehicleCondition( return true; } +ObjectData::Behavior getObjectBehavior( + ObjectData & object, const std::shared_ptr & parameters) +{ + if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { + return ObjectData::Behavior::NONE; + } + + return isMergingToEgoLane(object) ? ObjectData::Behavior::MERGING + : ObjectData::Behavior::DEVIATING; +} + bool isSatisfiedWithVehicleCondition( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, @@ -586,6 +744,7 @@ bool isSatisfiedWithVehicleCondition( { using boost::geometry::within; + object.behavior = getObjectBehavior(object, parameters); object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); // from here condition check for vehicle type objects. @@ -593,12 +752,7 @@ bool isSatisfiedWithVehicleCondition( return true; } - // Object is on center line -> ignore. - if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; - return false; - } - + // check vehicle shift ratio lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); const auto on_ego_driving_lane = within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); @@ -611,12 +765,29 @@ bool isSatisfiedWithVehicleCondition( } } - if (!object.is_within_intersection) { - return true; + // 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; } - if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { - object.reason = "ParallelToEgoLane"; + if (object.is_within_intersection) { + std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "straight") { + return true; + } + + if (object.behavior == ObjectData::Behavior::NONE) { + object.reason = "ParallelToEgoLane"; + return false; + } + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; return false; } @@ -674,11 +845,73 @@ 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) { - return obj.lateral < 0.0; + if (obj.direction == Direction::NONE) { + throw std::logic_error("object direction is not initialized. something wrong."); + } + + return obj.direction == Direction::RIGHT; } double calcShiftLength( @@ -756,11 +989,21 @@ size_t findPathIndexFromArclength( std::vector concatParentIds(const std::vector & ids1, const std::vector & ids2) { std::vector ret; - for (const auto id2 : ids2) { - if (std::any_of(ids1.begin(), ids1.end(), [&id2](const auto & id1) { return id1 == id2; })) { + + for (const auto & id : ids1) { + if (std::any_of( + ret.begin(), ret.end(), [&id](const auto & exist_id) { return exist_id == id; })) { continue; } - ret.push_back(id2); + ret.push_back(id); + } + + for (const auto & id : ids2) { + if (std::any_of( + ret.begin(), ret.end(), [&id](const auto & exist_id) { return exist_id == id; })) { + continue; + } + ret.push_back(id); } return ret; @@ -950,18 +1193,12 @@ std::vector generateObstaclePolygonsForDrivableArea( object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - const bool is_left = 0 < object.lateral; obstacles_for_drivable_area.push_back( - {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, is_left}); + {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); } 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) @@ -1222,6 +1459,22 @@ void fillAvoidanceNecessity( object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects) +{ + const auto id = object_data.object.object_id; + const auto same_id_obj = std::find_if( + detected_objects.begin(), detected_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + if (same_id_obj != detected_objects.end()) { + object_data.init_pose = same_id_obj->init_pose; + return; + } + + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + detected_objects.push_back(object_data); +} + void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) @@ -1345,83 +1598,8 @@ void compensateDetectionLost( } } -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) -{ - using lanelet::utils::to2D; - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = - findNearestIndex(data.reference_path_rough.points, object_pose.position); - const auto object_closest_pose = - data.reference_path_rough.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; - } - - double road_shoulder_distance = std::numeric_limits::max(); - - const bool get_left = isOnRight(object) && parameters->use_adjacent_lane; - const bool get_right = !isOnRight(object) && parameters->use_adjacent_lane; - const bool get_opposite = parameters->use_opposite_lane; - - lanelet::BasicPoint3d p_overhang( - object.overhang_pose.position.x, object.overhang_pose.position.y, - object.overhang_pose.position.z); - - lanelet::ConstLineString3d target_line{}; - - const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { - const auto lines = - rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); - const auto & line = isOnRight(object) ? lines.back() : lines.front(); - const auto d = boost::geometry::distance(object.envelope_poly, to2D(line.basicLineString())); - if (d < road_shoulder_distance) { - road_shoulder_distance = d; - target_line = line; - } - }; - - // current lanelet - { - update_road_to_shoulder_distance(object.overhang_lanelet); - - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, object.overhang_lanelet, - object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } - - // previous lanelet - lanelet::ConstLanelets previous_lanelet{}; - if (rh->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &previous_lanelet)) { - update_road_to_shoulder_distance(previous_lanelet.front()); - - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, previous_lanelet.front(), - object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } - - // next lanelet - lanelet::ConstLanelet next_lanelet{}; - if (rh->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { - update_road_to_shoulder_distance(next_lanelet); - - road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, road_shoulder_distance, next_lanelet, object.overhang_pose.position, - p_overhang, parameters->use_hatched_road_markings, parameters->use_intersection_areas); - } - - return road_shoulder_distance; -} - 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) { @@ -1436,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)) { @@ -1465,93 +1647,6 @@ void filterTargetObjects( } } -double extendToRoadShoulderDistanceWithPolygon( - const std::shared_ptr & rh, - const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, - const bool use_intersection_areas) -{ - // get expandable polygons for avoidance (e.g. hatched road markings) - std::vector expandable_polygons; - - const auto exist_polygon = [&](const auto & candidate_polygon) { - return std::any_of( - expandable_polygons.begin(), expandable_polygons.end(), - [&](const auto & polygon) { return polygon.id() == candidate_polygon.id(); }); - }; - - if (use_hatched_road_markings) { - for (const auto & point : target_line) { - const auto new_polygon_candidate = - utils::getPolygonByPoint(rh, point, "hatched_road_markings"); - - if (!!new_polygon_candidate && !exist_polygon(*new_polygon_candidate)) { - expandable_polygons.push_back(*new_polygon_candidate); - } - } - } - - if (use_intersection_areas) { - const std::string area_id_str = overhang_lanelet.attributeOr("intersection_area", "else"); - - if (area_id_str != "else") { - expandable_polygons.push_back( - rh->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id_str.c_str()))); - } - } - - if (expandable_polygons.empty()) { - return to_road_shoulder_distance; - } - - // calculate point laterally offset from overhang position to calculate intersection with - // polygon - Point lat_offset_overhang_pos; - { - auto arc_coordinates = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(target_line), lanelet::utils::to2D(overhang_basic_pose)); - arc_coordinates.distance = 0.0; - const auto closest_target_line_point = - lanelet::geometry::fromArcCoordinates(target_line, arc_coordinates); - - const double ratio = 100.0 / to_road_shoulder_distance; - lat_offset_overhang_pos.x = - closest_target_line_point.x() + (closest_target_line_point.x() - overhang_pos.x) * ratio; - lat_offset_overhang_pos.y = - closest_target_line_point.y() + (closest_target_line_point.y() - overhang_pos.y) * ratio; - } - - // update to_road_shoulder_distance with valid expandable polygon - double updated_to_road_shoulder_distance = to_road_shoulder_distance; - for (const auto & polygon : expandable_polygons) { - std::vector intersect_dist_vec; - for (size_t i = 0; i < polygon.size(); ++i) { - const auto polygon_current_point = - geometry_msgs::build().x(polygon[i].x()).y(polygon[i].y()).z(0.0); - const auto polygon_next_point = geometry_msgs::build() - .x(polygon[(i + 1) % polygon.size()].x()) - .y(polygon[(i + 1) % polygon.size()].y()) - .z(0.0); - - const auto intersect_pos = tier4_autoware_utils::intersect( - overhang_pos, lat_offset_overhang_pos, polygon_current_point, polygon_next_point); - if (intersect_pos) { - intersect_dist_vec.push_back(calcDistance2d(*intersect_pos, overhang_pos)); - } - } - - if (intersect_dist_vec.empty()) { - continue; - } - - std::sort(intersect_dist_vec.begin(), intersect_dist_vec.end()); - updated_to_road_shoulder_distance = - std::max(updated_to_road_shoulder_distance, intersect_dist_vec.back()); - } - return updated_to_road_shoulder_distance; -} - AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line) { AvoidLineArray ret{line}; @@ -1593,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); @@ -1675,12 +1772,12 @@ lanelet::ConstLanelets getAdjacentLane( lanelet::ConstLanelets lanes{}; for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); + const auto opt_left_lane = rh->getLeftLanelet(lane, true, false); if (!is_right_shift && opt_left_lane) { lanes.push_back(opt_left_lane.value()); } - const auto opt_right_lane = rh->getRightLanelet(lane); + const auto opt_right_lane = rh->getRightLanelet(lane, true, false); if (is_right_shift && opt_right_lane) { lanes.push_back(opt_right_lane.value()); } @@ -1696,7 +1793,8 @@ lanelet::ConstLanelets getAdjacentLane( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift) + const std::shared_ptr & parameters, const bool is_right_shift, + DebugData & debug) { const auto & p = parameters; const auto check_right_lanes = @@ -1754,6 +1852,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check left lanes @@ -1773,6 +1874,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check current lanes @@ -1792,19 +1896,27 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } return target_objects; } 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; @@ -1815,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); @@ -1873,7 +1989,7 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters, const bool in_avoidance_maneuver) { const auto & route_handler = planner_data->route_handler; @@ -1887,6 +2003,11 @@ DrivableLanes generateExpandDrivableLanes( // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_left_lanelets.empty()) { @@ -1897,6 +2018,11 @@ DrivableLanes generateExpandDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto next_lanes = route_handler->getNextLanelets(target_lane); + const auto is_stop_signal = utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data); + if (is_stop_signal && !in_avoidance_maneuver) { + return; + } const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( target_lane, parameters->use_opposite_lane, true); if (!all_right_lanelets.empty()) { @@ -2056,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 = @@ -2068,4 +2196,100 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } + +TurnSignalInfo calcTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, + const AvoidancePlanningData & data, const std::shared_ptr & planner_data) +{ + constexpr double THRESHOLD = 0.1; + const auto & p = planner_data->parameters; + const auto & rh = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x; + + if (shift_line.start_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.start_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.end_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + if (shift_line.end_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return {}; + } + + const auto start_shift_length = path.shift_length.at(shift_line.start_idx); + const auto end_shift_length = path.shift_length.at(shift_line.end_idx); + const auto relative_shift_length = end_shift_length - start_shift_length; + + // If shift length is shorter than the threshold, it does not need to turn on blinkers + if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { + return {}; + } + + // If the vehicle does not shift anymore, we turn off the blinker + if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) { + return {}; + } + + const auto get_command = [](const auto & shift_length) { + return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT + : TurnIndicatorsCommand::ENABLE_RIGHT; + }; + + const auto signal_prepare_distance = + std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); + const auto ego_front_to_shift_start = + calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - + p.vehicle_info.max_longitudinal_offset_m; + + if (signal_prepare_distance < ego_front_to_shift_start) { + return {}; + } + + const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; + const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; + const auto get_start_pose = [&](const auto & ego_to_shift_start) { + return ego_to_shift_start ? ego_pose : blinker_start_pose; + }; + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); + turn_signal_info.desired_end_point = blinker_end_pose; + turn_signal_info.required_start_point = blinker_start_pose; + turn_signal_info.required_end_point = blinker_end_pose; + turn_signal_info.turn_signal.command = get_command(relative_shift_length); + + if (!p.turn_signal_on_swerving) { + return turn_signal_info; + } + + lanelet::ConstLanelet lanelet; + if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { + return {}; + } + + const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true); + const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true); + + if (!existShiftSideLane( + start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) { + return {}; + } + + if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) { + return {}; + } + + return turn_signal_info; +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 83514127b0a8e..8cf85554e2b2c 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,14 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" -#include +#include +#include +#include -#include #include using behavior_path_planner::BehaviorPathPlannerNode; @@ -62,8 +60,6 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_planner") + - "/config/lane_change/lane_change.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + "/config/avoidance.param.yaml"}); diff --git a/planning/behavior_path_avoidance_module/test/test_utils.cpp b/planning/behavior_path_avoidance_module/test/test_utils.cpp index e5c6fd2e72092..95488daa69cf8 100644 --- a/planning/behavior_path_avoidance_module/test/test_utils.cpp +++ b/planning/behavior_path_avoidance_module/test/test_utils.cpp @@ -25,7 +25,7 @@ using behavior_path_planner::utils::avoidance::isSameDirectionShift; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { ObjectData right_obj; - right_obj.lateral = -0.3; + right_obj.direction = route_handler::Direction::RIGHT; const double negative_shift_length = -1.0; const double positive_shift_length = 1.0; @@ -33,11 +33,7 @@ TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), positive_shift_length)); ObjectData left_obj; - left_obj.lateral = 0.3; + left_obj.direction = route_handler::Direction::LEFT; ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), positive_shift_length)); ASSERT_FALSE(isSameDirectionShift(isOnRight(left_obj), negative_shift_length)); - - const double zero_shift_length = 0.0; - ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), zero_shift_length)); - ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), zero_shift_length)); } diff --git a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt new file mode 100644 index 0000000000000..98874b8923324 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_dynamic_avoidance_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_dynamic_avoidance_module/README.md similarity index 94% rename from planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md rename to planning/behavior_path_dynamic_avoidance_module/README.md index f303937bd0f78..5e8be9136886c 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md +++ b/planning/behavior_path_dynamic_avoidance_module/README.md @@ -32,16 +32,16 @@ First, a maximum lateral offset to avoid is calculated as follows. The polygon's width to extract from the drivable area is the obstacle width and double `drivable_area_generation.lat_offset_from_obstacle`. We can limit the lateral shift offset by `drivable_area_generation.max_lat_offset_to_avoid`. -![drivable_area_extraction_width](../image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg) +![drivable_area_extraction_width](./image/drivable_area_extraction_width.drawio.svg) Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision). Regarding the same directional obstacles, obstacles whose TTC is negative will be ignored (e.g. The obstacle is in front of the ego, and the obstacle's velocity is larger than the ego's velocity.). Same directional obstacles -![same_directional_object](../image/dynamic_avoidance/same_directional_object.svg) +![same_directional_object](./image/same_directional_object.svg) Opposite directional obstacles -![opposite_directional_object](../image/dynamic_avoidance/opposite_directional_object.svg) +![opposite_directional_object](./image/opposite_directional_object.svg) ## Parameters diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml similarity index 89% rename from planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml rename to planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml index 854c29aa89bc5..371f7da2fadf5 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml @@ -46,6 +46,9 @@ min_object_vel: -0.5 # [m/s] The value is negative considering the noisy velocity of the stopped vehicle. max_ego_path_lat_cover_ratio: 0.3 # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value. + stopped_object: + max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value. + drivable_area_generation: polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base" object_path_base: @@ -56,6 +59,10 @@ max_time_for_object_lat_shift: 0.0 # [s] lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] + max_ego_lat_acc: 0.3 # [m/ss] + max_ego_lat_jerk: 0.3 # [m/sss] + delay_time_ego_shift: 1.0 # [s] + # for same directional object overtaking_object: max_time_to_collision: 40.0 # [s] diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg b/planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg rename to planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg b/planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg similarity index 100% rename from planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg rename to planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp index c02ee88d3fa3e..c2c17c4e55c1e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -44,10 +44,12 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + private: std::shared_ptr parameters_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp similarity index 71% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp rename to planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp index e37ef27d44426..a5380b628387d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -23,9 +23,6 @@ #include #include #include -#include -#include -#include #include #include @@ -35,6 +32,25 @@ #include #include +namespace +{ +template +bool isInVector(const T & val, const std::vector & vec) +{ + return std::find(vec.begin(), vec.end(), val) != vec.end(); +} + +template +std::vector getAllKeys(const std::unordered_map & map) +{ + std::vector keys; + for (const auto & pair : map) { + keys.push_back(pair.first); + } + return keys; +} +} // namespace + namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedPath; @@ -87,6 +103,7 @@ struct DynamicAvoidanceParameters double max_overtaking_crossing_object_angle{0.0}; double min_oncoming_crossing_object_vel{0.0}; double max_oncoming_crossing_object_angle{0.0}; + double max_stopped_object_vel{0.0}; // drivable area generation PolygonGenerationMethod polygon_generation_method{}; @@ -96,6 +113,10 @@ struct DynamicAvoidanceParameters double max_time_for_lat_shift{0.0}; double lpf_gain_for_lat_avoid_to_offset{0.0}; + double max_ego_lat_acc{0.0}; + double max_ego_lat_jerk{0.0}; + double delay_time_ego_shift{0.0}; + double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; double end_duration_to_avoid_overtaking_object{0.0}; @@ -106,6 +127,17 @@ struct DynamicAvoidanceParameters double end_duration_to_avoid_oncoming_object{0.0}; }; +struct TimeWhileCollision +{ + double time_to_start_collision; + double time_to_end_collision; +}; + +struct LatFeasiblePaths +{ + std::vector left_path; + std::vector right_path; +}; class DynamicAvoidanceModule : public SceneModuleInterface { public: @@ -143,15 +175,19 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left{false}; bool should_be_avoided{false}; + std::vector ref_path_points_for_obj_poly; + LatFeasiblePaths ego_lat_feasible_paths; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided) + const bool arg_is_collision_left, const bool arg_should_be_avoided, + const std::vector & arg_ref_path_points_for_obj_poly) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; + ref_path_points_for_obj_poly = arg_ref_path_points_for_obj_poly; } }; @@ -162,14 +198,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface { } int max_count_{0}; - int min_count_{0}; + int min_count_{0}; // TODO(murooka): The sign needs to be opposite? void initialize() { current_uuids_.clear(); } void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) { // add/update object if (object_map_.count(uuid) != 0) { + const auto prev_object = object_map_.at(uuid); object_map_.at(uuid) = object; + // TODO(murooka) refactor this. Counter can be moved to DynamicObject, + // and TargetObjectsManager can be removed. + object_map_.at(uuid).ego_lat_feasible_paths = prev_object.ego_lat_feasible_paths; } else { object_map_.emplace(uuid, object); } @@ -185,14 +225,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface current_uuids_.push_back(uuid); } - void finalize() + void finalize(const LatFeasiblePaths & ego_lat_feasible_paths) { // decrease counter for not updated uuids std::vector not_updated_uuids; for (const auto & object : object_map_) { - if ( - std::find(current_uuids_.begin(), current_uuids_.end(), object.first) == - current_uuids_.end()) { + if (!isInVector(object.first, current_uuids_)) { not_updated_uuids.push_back(object.first); } } @@ -204,52 +242,55 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - // remove objects whose counter is lower than threshold - std::vector obsolete_uuids; + // update valid object uuids and its variable for (const auto & counter : counter_map_) { - if (counter.second < min_count_) { - obsolete_uuids.push_back(counter.first); + if (!isInVector(counter.first, valid_object_uuids_) && max_count_ <= counter.second) { + valid_object_uuids_.push_back(counter.first); + object_map_.at(counter.first).ego_lat_feasible_paths = ego_lat_feasible_paths; } } - for (const auto & obsolete_uuid : obsolete_uuids) { - counter_map_.erase(obsolete_uuid); - object_map_.erase(obsolete_uuid); + valid_object_uuids_.erase( + std::remove_if( + valid_object_uuids_.begin(), valid_object_uuids_.end(), + [&](const auto & uuid) { + return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_; + }), + valid_object_uuids_.end()); + + // remove objects whose counter is lower than threshold + const auto counter_map_keys = getAllKeys(counter_map_); + for (const auto & key : counter_map_keys) { + if (counter_map_.at(key) < min_count_) { + counter_map_.erase(key); + object_map_.erase(key); + } } } std::vector getValidObjects() const { - std::vector objects; - for (const auto & object : object_map_) { - if (counter_map_.count(object.first) != 0) { - if (max_count_ <= counter_map_.at(object.first)) { - objects.push_back(object.second); - } + std::vector valid_objects; + for (const auto & valid_object_uuid : valid_object_uuids_) { + if (object_map_.count(valid_object_uuid) == 0) { + std::cerr + << "[DynamicAvoidance] Internal calculation has an error when getting valid objects." + << std::endl; + continue; } + valid_objects.push_back(object_map_.at(valid_object_uuid)); } - return objects; - } - std::optional getValidObject(const std::string & uuid) const - { - // add/update object - if (counter_map_.count(uuid) == 0) { - return std::nullopt; - } - if (counter_map_.at(uuid) < max_count_) { - return std::nullopt; - } - if (object_map_.count(uuid) == 0) { - return std::nullopt; - } - return object_map_.at(uuid); + + return valid_objects; } - void updateObject( + void updateObjectVariables( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided) + const bool should_be_avoided, + const std::vector & ref_path_points_for_obj_poly) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + ref_path_points_for_obj_poly); } } @@ -257,6 +298,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface // NOTE: positive is for meeting entry condition, and negative is for exiting. std::unordered_map counter_map_; std::unordered_map object_map_; + std::vector valid_object_uuids_{}; }; struct DecisionWithReason @@ -307,33 +349,41 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); + LatFeasiblePaths generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const; + void updateRefPathBeforeLaneChange(const std::vector & ego_ref_path_points); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; + bool willObjectBeOutsideEgoChangingPath( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; - double calcTimeToCollision( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + TimeWhileCollision calcTimeWhileCollision( + const std::vector & ego_path, const double obj_tangent_vel, + const LatLonOffset & lat_lon_offset) const; std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; - double calcValidStartLengthToAvoid( + double calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const bool is_object_same_direction) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & input_ref_path_points, + const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const double time_to_collision) const; + const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoid( - const std::vector & input_ref_path_points, const Polygon2d & obj_points, - const double obj_vel, const bool is_collision_left, const double obj_normal_vel, + const std::vector & ref_path_points_for_obj_poly, + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, const std::optional & prev_object) const; std::pair getAdjacentLanes( @@ -353,11 +403,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::vector target_objects_; // std::vector prev_target_objects_; - std::vector prev_input_ref_path_points; + std::optional> prev_input_ref_path_points_{std::nullopt}; + std::optional> ref_path_before_lane_change_{std::nullopt}; std::shared_ptr parameters_; TargetObjectsManager target_objects_manager_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ +#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/package.xml b/planning/behavior_path_dynamic_avoidance_module/package.xml new file mode 100644 index 0000000000000..11792a15b2cd2 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_path_dynamic_avoidance_module + 0.1.0 + The behavior_path_dynamic_avoidance_module package + + Takayuki Murooka + Yuki Takagi + Satoshi Ota + Kosuke Takeuchi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + lanelet2_core + lanelet2_extension + motion_utils + object_recognition_utils + pluginlib + rclcpp + signal_processing + tier4_autoware_utils + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_dynamic_avoidance_module/plugins.xml b/planning/behavior_path_dynamic_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..fd2e1bc4137b7 --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp similarity index 93% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/manager.cpp index 2824a6221591a..569c6a17b5b32 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_dynamic_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -99,6 +99,9 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "crossing_object.min_oncoming_object_vel"); p.max_oncoming_crossing_object_angle = node->declare_parameter(ns + "crossing_object.max_oncoming_object_angle"); + + p.max_stopped_object_vel = + node->declare_parameter(ns + "stopped_object.max_object_vel"); } { // drivable_area_generation @@ -114,6 +117,10 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) p.lpf_gain_for_lat_avoid_to_offset = node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); + p.max_ego_lat_acc = node->declare_parameter(ns + "max_ego_lat_acc"); + p.max_ego_lat_jerk = node->declare_parameter(ns + "max_ego_lat_jerk"); + p.delay_time_ego_shift = node->declare_parameter(ns + "delay_time_ego_shift"); + p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); p.start_duration_to_avoid_overtaking_object = @@ -207,6 +214,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "crossing_object.max_oncoming_object_angle", p->max_oncoming_crossing_object_angle); + + updateParam( + parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel); } { // drivable_area_generation @@ -227,6 +237,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); + updateParam(parameters, ns + "max_ego_lat_acc", p->max_ego_lat_acc); + updateParam(parameters, ns + "max_ego_lat_jerk", p->max_ego_lat_jerk); + updateParam(parameters, ns + "delay_time_ego_shift", p->delay_time_ego_shift); + updateParam( parameters, ns + "overtaking_object.max_time_to_collision", p->max_time_to_collision_overtaking_object); @@ -255,6 +269,11 @@ void DynamicAvoidanceModuleManager::updateModuleParams( if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } + +bool DynamicAvoidanceModuleManager::isAlwaysExecutableModule() const +{ + return true; +} } // namespace behavior_path_planner #include diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp similarity index 63% rename from planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp rename to planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index de76166e068d3..b43102ecbd16d 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" +#include "behavior_path_dynamic_avoidance_module/scene.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -137,7 +137,26 @@ double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & s return max_length_to_point; } - throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); + throw std::logic_error("The shape type is not supported in dynamic_avoidance."); +} + +double calcObstacleWidth(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return shape.dimensions.y; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + throw std::logic_error("The shape type is not supported in dynamic_avoidance."); } double calcDiffAngleAgainstPath( @@ -153,7 +172,7 @@ double calcDiffAngleAgainstPath( return diff_yaw; } -double calcDiffAngleBetweenPaths( +[[maybe_unused]] double calcDiffAngleBetweenPaths( const std::vector & path_points, const PredictedPath & predicted_path) { const size_t nearest_idx = @@ -243,6 +262,35 @@ double calcDistanceToSegment( first_to_second * first_to_target.dot(first_to_second) / std::pow(first_to_second.norm(), 2); return (Eigen::Vector2d{p1.x, p1.y} - p2_nearest).norm(); } + +std::optional> intersectLines( + const std::vector & source_line, + const std::vector & target_line) +{ + for (int source_seg_idx = 0; source_seg_idx < static_cast(source_line.size()) - 1; + ++source_seg_idx) { + for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; + ++target_seg_idx) { + const auto intersect_point = tier4_autoware_utils::intersect( + source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, + target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); + if (intersect_point) { + return std::make_pair(source_seg_idx, *intersect_point); + } + } + } + return std::nullopt; +} + +std::vector convertToPoints( + const std::vector & poses) +{ + std::vector points; + for (const auto & pose : poses) { + points.push_back(pose.position); + } + return points; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( @@ -264,12 +312,12 @@ bool DynamicAvoidanceModule::isExecutionRequested() const RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); const auto input_path = getPreviousModuleOutput().path; - if (!input_path || input_path->points.size() < 2) { + if (input_path.points.size() < 2) { return false; } // check if the ego is driving forward - const auto is_driving_forward = motion_utils::isDrivingForward(input_path->points); + const auto is_driving_forward = motion_utils::isDrivingForward(input_path.points); if (!is_driving_forward || !(*is_driving_forward)) { return false; } @@ -291,6 +339,9 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { + info_marker_.markers.clear(); + debug_marker_.markers.clear(); + // calculate target objects updateTargetObjects(); @@ -310,9 +361,6 @@ bool DynamicAvoidanceModule::canTransitSuccessState() BehaviorModuleOutput DynamicAvoidanceModule::plan() { - info_marker_.markers.clear(); - debug_marker_.markers.clear(); - const auto & input_path = getPreviousModuleOutput().path; // create obstacles to avoid (= extract from the drivable area) @@ -401,9 +449,15 @@ void DynamicAvoidanceModule::updateTargetObjects() const auto input_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + const auto input_ref_path_points = getPreviousModuleOutput().reference_path.points; const auto prev_objects = target_objects_manager_.getValidObjects(); + updateRefPathBeforeLaneChange(input_ref_path_points); + + const auto & ego_pose = getEgoPose(); + const double ego_vel = getEgoSpeed(); + const auto ego_lat_feasible_paths = generateLateralFeasiblePaths(ego_pose, ego_vel); + // 1. Rough filtering of target objects target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { @@ -427,7 +481,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = - projectObstacleVelocityToTrajectory(input_path->points, predicted_object); + projectObstacleVelocityToTrajectory(input_path.points, predicted_object); if ( std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { @@ -435,7 +489,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleBetweenPaths(input_path->points, obj_path); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, obj_pose); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -455,7 +509,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.e. check if object lateral offset to ego's path is small enough - const double obj_dist_to_path = calcDistanceToPath(input_path->points, obj_pose.position); + const double obj_dist_to_path = calcDistanceToPath(input_path.points, obj_pose.position); const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); if (is_object_far_from_path) { RCLCPP_INFO_EXPRESSION( @@ -488,7 +542,7 @@ void DynamicAvoidanceModule::updateTargetObjects() latest_time_inside_ego_path); target_objects_manager_.updateObject(obj_uuid, target_object); } - target_objects_manager_.finalize(); + target_objects_manager_.finalize(ego_lat_feasible_paths); // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { @@ -498,8 +552,10 @@ void DynamicAvoidanceModule::updateTargetObjects() object.predicted_paths.begin(), object.predicted_paths.end(), [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const auto & ref_path_points_for_obj_poly = input_path.points; + // 2.a. check if object is not to be followed by ego - const double obj_angle = calcDiffAngleAgainstPath(input_path->points, object.pose); + const double obj_angle = calcDiffAngleAgainstPath(input_path.points, object.pose); const bool is_object_aligned_to_path = std::abs(obj_angle) < parameters_->max_front_object_angle || M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); @@ -513,13 +569,13 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.b. calculate which side object exists against ego's path - const bool is_object_left = isLeft(input_path->points, object.pose.position); + const bool is_object_left = isLeft(input_path.points, object.pose.position); const auto lat_lon_offset = - getLateralLongitudinalOffset(input_path->points, object.pose, object.shape); + getLateralLongitudinalOffset(input_path.points, object.pose, object.shape); // 2.c. check if object will not cut in const bool will_object_cut_in = - willObjectCutIn(input_path->points, obj_path, object.vel, lat_lon_offset); + willObjectCutIn(input_path.points, obj_path, object.vel, lat_lon_offset); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -535,39 +591,66 @@ void DynamicAvoidanceModule::updateTargetObjects() continue; } + // 2.e. check if the ego will change the lane and the object will be outside the ego's path + // const auto will_object_be_outside_ego_changing_path = + // willObjectBeOutsideEgoChangingPath(object.pose, object.shape, object.vel); + // if (will_object_be_outside_ego_changing_path) { + // RCLCPP_INFO_EXPRESSION( + // getLogger(), parameters_->enable_debug_info, + // "[DynamicAvoidance] Ignore obstacle (%s) since the object will be outside ego's changing + // path", obj_uuid.c_str()); + // continue; + // } + // 2.e. check time to collision - const double time_to_collision = - calcTimeToCollision(input_path->points, object.pose, object.vel, lat_lon_offset); - if ( - (0 <= object.vel && - parameters_->max_time_to_collision_overtaking_object < time_to_collision) || - (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", - obj_uuid.c_str(), time_to_collision); - continue; - } - if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { - RCLCPP_INFO_EXPRESSION( - getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative " - "value.", - obj_uuid.c_str(), time_to_collision); - continue; + const auto time_while_collision = + calcTimeWhileCollision(input_path.points, object.vel, lat_lon_offset); + const double time_to_collision = time_while_collision.time_to_start_collision; + if (parameters_->max_stopped_object_vel < std::hypot(object.vel, object.lat_vel)) { + // NOTE: Only not stopped object is filtered by time to collision. + if ( + (0 <= object.vel && + parameters_->max_time_to_collision_overtaking_object < time_to_collision) || + (object.vel <= 0 && + parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is large.", + obj_uuid.c_str(), time_to_collision_str.c_str()); + continue; + } + if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is a small " + "negative value.", + obj_uuid.c_str(), time_to_collision_str.c_str()); + continue; + } } // 2.f. calculate which side object will be against ego's path - const auto future_obj_pose = - object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); - const bool is_collision_left = - future_obj_pose ? isLeft(input_path->points, future_obj_pose->position) : is_object_left; + const bool is_collision_left = [&]() { + if (0.0 < object.vel) { + return is_object_left; + } + const auto future_obj_pose = + object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); + return future_obj_pose ? isLeft(input_path.points, future_obj_pose->position) + : is_object_left; + }(); // 2.g. check if the ego is not ahead of the object. const double signed_dist_ego_to_obj = [&]() { - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path->points); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( - input_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_path.points, ego_pose.position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -590,11 +673,11 @@ void DynamicAvoidanceModule::updateTargetObjects() // "ego_path_base" const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - input_ref_path_points, object.pose, obj_points, object.vel, obj_path, object.shape, - time_to_collision); + 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( - input_ref_path_points, 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, @@ -605,11 +688,74 @@ void DynamicAvoidanceModule::updateTargetObjects() } const bool should_be_avoided = true; - target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided); + target_objects_manager_.updateObjectVariables( + obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, + ref_path_points_for_obj_poly); + } + + prev_input_ref_path_points_ = input_ref_path_points; +} + +LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const +{ + const double lat_acc = parameters_->max_ego_lat_acc; + const double lat_jerk = parameters_->max_ego_lat_jerk; + const double delay_time = parameters_->delay_time_ego_shift; + + LatFeasiblePaths ego_lat_feasible_paths; + for (double t = 0; t < 10.0; t += 1.0) { + const double feasible_lat_offset = lat_acc * std::pow(std::max(t - delay_time, 0.0), 2) / 2.0 + + lat_jerk * std::pow(std::max(t - delay_time, 0.0), 3) / 6.0 - + planner_data_->parameters.vehicle_width / 2.0; + const double x = t * ego_vel; + const double y = feasible_lat_offset; + + const auto feasible_left_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); + + const auto feasible_right_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); } - prev_input_ref_path_points = input_ref_path_points; + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + + return ego_lat_feasible_paths; +} + +void DynamicAvoidanceModule::updateRefPathBeforeLaneChange( + const std::vector & ego_ref_path_points) +{ + if (ref_path_before_lane_change_) { + // check if the ego is close enough to the current ref path, meaning that lane change ends. + const auto ego_pos = getEgoPose().position; + const double dist_to_ref_path = + std::abs(motion_utils::calcLateralOffset(ego_ref_path_points, ego_pos)); + + constexpr double epsilon_dist_to_ref_path = 0.5; + if (dist_to_ref_path < epsilon_dist_to_ref_path) { + ref_path_before_lane_change_ = std::nullopt; + } + } else { + // check if the ego is during lane change. + if (prev_input_ref_path_points_ && !prev_input_ref_path_points_->empty()) { + const double dist_ref_paths = std::abs(motion_utils::calcLateralOffset( + ego_ref_path_points, prev_input_ref_path_points_->front().point.pose.position)); + constexpr double epsilon_ref_paths_diff = 1.0; + if (epsilon_ref_paths_diff < dist_ref_paths) { + ref_path_before_lane_change_ = *prev_input_ref_path_points_; + } + } + } } [[maybe_unused]] std::optional> @@ -650,26 +796,60 @@ DynamicAvoidanceModule::calcCollisionSection( return std::make_pair(*collision_start_idx, ego_path.size() - 1); } -double DynamicAvoidanceModule::calcTimeToCollision( - const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const +TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( + const std::vector & ego_path, const double obj_tangent_vel, + const LatLonOffset & lat_lon_offset) const { // Set maximum time-to-collision 0 if the object longitudinally overlaps ego. // NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative. const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); - const double lon_offset_ego_to_obj = - motion_utils::calcSignedArcLength( - ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) + - lat_lon_offset.max_lon_offset; - const double maximum_time_to_collision = - (0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits::max(); - + const double lon_offset_ego_to_obj_idx = motion_utils::calcSignedArcLength( + ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; - const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); - const double signed_lon_length = motion_utils::calcSignedArcLength( - ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx); - const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity); + + 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.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; + if (0.0 < lon_offset_ego_front_to_obj_back) { // The object is ahead of the ego. + return lon_offset_ego_front_to_obj_back / relative_velocity; + } else if (0.0 < lon_offset_obj_front_to_ego_back) { // The ego is ahead of the object. + return lon_offset_obj_front_to_ego_back / -relative_velocity; + } + // The ego and object are colliding. + return 0.0; + }(); + const double signed_time_to_end_collision = [&]() { + const double lon_offset_ego_back_to_obj_front = lon_offset_ego_to_obj_idx + + lat_lon_offset.max_lon_offset + + planner_data_->parameters.rear_overhang; + const double lon_offset_obj_back_to_ego_front = -lon_offset_ego_to_obj_idx - + lat_lon_offset.min_lon_offset + + planner_data_->parameters.front_overhang; + if (0.0 < relative_velocity) { + return lon_offset_ego_back_to_obj_front / relative_velocity; + } + return lon_offset_obj_back_to_ego_front / -relative_velocity; + }(); + + // NOTE: In order to make time_to_start_collision continuous around the relative_velocity is zero. + const double time_to_start_collision = [&]() { + if (signed_time_to_start_collision < 0.0) { + return std::numeric_limits::max(); + } + return signed_time_to_start_collision; + }(); + const double time_to_end_collision = [&]() { + if (signed_time_to_end_collision < 0.0) { + return std::numeric_limits::max(); + } + return signed_time_to_end_collision; + }(); + + return TimeWhileCollision{time_to_start_collision, time_to_end_collision}; } bool DynamicAvoidanceModule::isObjectFarFromPath( @@ -765,6 +945,25 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut return DecisionWithReason{false}; } +[[maybe_unused]] bool DynamicAvoidanceModule::willObjectBeOutsideEgoChangingPath( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const +{ + if (!ref_path_before_lane_change_ || obj_vel < 0.0) { + return false; + } + + // Check if object is in the lane before ego's lane change. + const double dist_to_ref_path_before_lane_change = + std::abs(motion_utils::calcLateralOffset(*ref_path_before_lane_change_, obj_pose.position)); + const double epsilon_dist_checking_in_lane = calcObstacleWidth(obj_shape); + if (epsilon_dist_checking_in_lane < dist_to_ref_path_before_lane_change) { + return false; + } + + return true; +} + std::pair DynamicAvoidanceModule::getAdjacentLanes( const double forward_distance, const double backward_distance) const { @@ -840,13 +1039,13 @@ DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudi } MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & input_ref_path_points, + const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const double time_to_collision) const + const TimeWhileCollision & time_while_collision) const { const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, obj_pose.position); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position); // calculate min/max longitudinal offset from object to path const auto obj_lon_offset = [&]() { @@ -854,67 +1053,105 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( - input_ref_path_points, obj_seg_idx, geom_obj_point); + ref_path_points_for_obj_poly, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } - const auto raw_obj_lon_offset = getMinMaxValues(obj_lon_offset_vec); + return getMinMaxValues(obj_lon_offset_vec); + }(); - if (obj_vel < 0) { - return MinMaxValue{ - raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value}; - } + const double relative_velocity = getEgoSpeed() - obj_vel; - // NOTE: If time to collision is considered here, the ego is close to the object when starting - // avoidance. - // The ego should start avoidance before overtaking. - return raw_obj_lon_offset; + // calculate bound start and end length + const double start_length_to_avoid = [&]() { + if (obj_vel < parameters_->max_stopped_object_vel) { + // The ego and object are the same directional or the object is parked. + return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object; + } + // The ego and object are the opposite directional. + 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 / + obj_acc; + const double ego_moving_dist = getEgoSpeed() * decel_time; + return std::max(0.0, ego_moving_dist - obj_moving_dist) + + std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object; + }(); + const double end_length_to_avoid = [&]() { + if (obj_vel < parameters_->max_stopped_object_vel) { + // The ego and object are the same directional or the object is parked. + return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object; + } + // The ego and object are the opposite directional. + return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel + + std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object; }(); - // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= obj_vel); - // TODO(murooka) use getEgoSpeed() instead of obj_vel - const double start_length_to_avoid = - std::abs(obj_vel) * (is_object_overtaking - ? parameters_->start_duration_to_avoid_overtaking_object - : parameters_->start_duration_to_avoid_oncoming_object); - const double end_length_to_avoid = - std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); - - if (obj_vel < 0) { - const double valid_start_length_to_avoid = - calcValidStartLengthToAvoid(obj_path, obj_pose, obj_shape); + // calculate valid path for the forked object's path from the ego's path + if (obj_vel < -parameters_->max_stopped_object_vel) { + const bool is_object_same_direction = false; + const double valid_length_to_avoid = + calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction); return MinMaxValue{ - std::max(obj_lon_offset.min_value - start_length_to_avoid, valid_start_length_to_avoid), + obj_lon_offset.min_value + std::max(-start_length_to_avoid, -valid_length_to_avoid), obj_lon_offset.max_value + end_length_to_avoid}; } + if (parameters_->max_stopped_object_vel < obj_vel) { + const bool is_object_same_direction = true; + const double valid_length_to_avoid = + calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction); + return MinMaxValue{ + obj_lon_offset.min_value - start_length_to_avoid, + obj_lon_offset.max_value + std::min(end_length_to_avoid, valid_length_to_avoid)}; + } return MinMaxValue{ obj_lon_offset.min_value - start_length_to_avoid, obj_lon_offset.max_value + end_length_to_avoid}; } -double DynamicAvoidanceModule::calcValidStartLengthToAvoid( +double DynamicAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const + const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const bool is_object_same_direction) const { - const auto input_path_points = getPreviousModuleOutput().path->points; + const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); + 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; + if (is_object_same_direction) { + cropped_ego_path_points = std::vector{ + input_path_points.begin() + obj_seg_idx, input_path_points.end()}; + } else { + cropped_ego_path_points = std::vector{ + input_path_points.begin(), input_path_points.begin() + obj_seg_idx + 1 + 1}; + std::reverse(cropped_ego_path_points.begin(), cropped_ego_path_points.end()); + } + if (cropped_ego_path_points.size() < 2) { + return motion_utils::calcArcLength(obj_path.path); + } + + // calculate where the object's path will be forked from (= far from) the ego's path. + std::optional last_nearest_ego_path_seg_idx{std::nullopt}; const size_t valid_obj_path_end_idx = [&]() { - int ego_path_idx = obj_seg_idx + 1; + size_t ego_path_seg_idx = 0; for (size_t obj_path_idx = 0; obj_path_idx < obj_path.path.size(); ++obj_path_idx) { bool are_paths_close{false}; - for (; 0 < ego_path_idx; --ego_path_idx) { + for (; ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) { const double dist_to_segment = calcDistanceToSegment( obj_path.path.at(obj_path_idx).position, - input_path_points.at(ego_path_idx).point.pose.position, - input_path_points.at(ego_path_idx - 1).point.pose.position); - if ( - dist_to_segment < planner_data_->parameters.vehicle_width / 2.0 + - parameters_->lat_offset_from_obstacle + - calcObstacleMaxLength(obj_shape)) { + cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position, + cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position); + if (dist_to_segment < dist_threshold_paths) { + last_nearest_ego_path_seg_idx = ego_path_seg_idx; are_paths_close = true; break; } @@ -926,25 +1163,66 @@ double DynamicAvoidanceModule::calcValidStartLengthToAvoid( } return obj_path.path.size() - 1; }(); - return -motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); + + // calculate valid length to avoid + if (last_nearest_ego_path_seg_idx && valid_obj_path_end_idx != obj_path.path.size() - 1) { + const auto calc_min_dist = [&](const size_t arg_obj_path_idx) -> std::optional { + std::optional min_dist{std::nullopt}; + for (size_t ego_path_seg_idx = *last_nearest_ego_path_seg_idx; + ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) { + const double dist_to_segment = calcDistanceToSegment( + obj_path.path.at(arg_obj_path_idx).position, + cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position, + cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position); + if (!min_dist || dist_to_segment < *min_dist) { + min_dist = dist_to_segment; + } + if (min_dist && *min_dist < dist_to_segment) { + return *min_dist; + } + } + return min_dist; + }; + const size_t prev_valid_obj_path_end_idx = + (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx : valid_obj_path_end_idx - 1; + const size_t next_valid_obj_path_end_idx = + (valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx + 1 : valid_obj_path_end_idx; + const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx); + const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx); + if (prev_min_dist && next_min_dist) { + const double segment_length = tier4_autoware_utils::calcDistance2d( + obj_path.path.at(prev_valid_obj_path_end_idx), + obj_path.path.at(next_valid_obj_path_end_idx)); + const double partial_segment_length = segment_length * + (dist_threshold_paths - *prev_min_dist) / + (*next_min_dist - *prev_min_dist); + return motion_utils::calcSignedArcLength(obj_path.path, 0, prev_valid_obj_path_end_idx) + + partial_segment_length; + } + } + return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); } std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( - const std::vector & input_ref_path_points, const Polygon2d & obj_points, - const double obj_vel, const bool is_collision_left, const double obj_normal_vel, + const std::vector & ref_path_points_for_obj_poly, + const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, + const bool is_collision_left, const double obj_normal_vel, const std::optional & prev_object) const { const bool enable_lowpass_filter = [&]() { - if (prev_input_ref_path_points.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_input_ref_path_points, input_ref_path_points.front().point.pose.position); - constexpr double min_lane_change_path_lat_offset = 1.0; - if ( - motion_utils::calcLateralOffset( - prev_input_ref_path_points, input_ref_path_points.front().point.pose.position, - prev_front_seg_idx) < min_lane_change_path_lat_offset) { + 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 @@ -958,9 +1236,9 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, geom_obj_point); - const double obj_point_lat_offset = - motion_utils::calcLateralOffset(input_ref_path_points, geom_obj_point, obj_point_seg_idx); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, geom_obj_point); + const double obj_point_lat_offset = motion_utils::calcLateralOffset( + ref_path_points_for_obj_poly, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(obj_point_lat_offset); } return getMinMaxValues(obj_lat_abs_offset_vec); @@ -1027,19 +1305,19 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( return std::nullopt; } - auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; + auto ref_path_points_for_obj_poly = object.ref_path_points_for_obj_poly; const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(input_ref_path_points, object.pose.position); + motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, object.pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid->min_value, input_ref_path_points); + obj_seg_idx, object.lon_offset_to_avoid->min_value, ref_path_points_for_obj_poly); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, input_ref_path_points); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, ref_path_points_for_obj_poly); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -1049,33 +1327,76 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); const size_t lon_bound_end_idx = lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(input_ref_path_points.size() - 1); + : static_cast(ref_path_points_for_obj_poly.size() - 1); - // create inner/outer bound points - std::vector obj_inner_bound_points; - std::vector obj_outer_bound_points; + // create inner bound points + std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back( - tier4_autoware_utils::calcOffsetPose( - input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, 0.0) - .position); - obj_outer_bound_points.push_back( + // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. + obj_inner_bound_poses.push_back(tier4_autoware_utils::calcOffsetPose( + ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, + 0.0)); + } + + // calculate start index laterally feasible for ego to shift considering maximum lateral jerk and + // acceleration + const auto obj_inner_bound_start_idx = [&]() -> std::optional { + const auto & ego_lat_feasible_path = object.is_collision_left + ? object.ego_lat_feasible_paths.left_path + : object.ego_lat_feasible_paths.right_path; + const auto intersect_result = intersectLines(obj_inner_bound_poses, ego_lat_feasible_path); + + // Check if the object polygon intersects with the ego_lat_feasible_path. + if (intersect_result) { + const auto & [bound_seg_idx, intersect_point] = *intersect_result; + const double lon_offset = tier4_autoware_utils::calcDistance2d( + obj_inner_bound_poses.at(bound_seg_idx), intersect_point); + + const auto obj_inner_bound_start_idx_opt = + motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); + if (obj_inner_bound_start_idx_opt) { + return *obj_inner_bound_start_idx_opt; + } + } + + // Check if the object polygon is fully outside the ego_lat_feasible_path. + const double obj_poly_lat_offset = motion_utils::calcLateralOffset( + ego_lat_feasible_path, obj_inner_bound_poses.front().position); + if ( + (!object.is_collision_left && 0 < obj_poly_lat_offset) || + (object.is_collision_left && obj_poly_lat_offset < 0)) { + return std::nullopt; + } + + return 0; + }(); + if (!obj_inner_bound_start_idx) { + return std::nullopt; + } + + // calculate feasible inner/outer bound points + const auto feasible_obj_inner_bound_poses = std::vector( + obj_inner_bound_poses.begin() + *obj_inner_bound_start_idx, obj_inner_bound_poses.end()); + const auto feasible_obj_inner_bound_points = convertToPoints(feasible_obj_inner_bound_poses); + std::vector feasible_obj_outer_bound_points; + for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { + feasible_obj_outer_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( - input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->max_value, 0.0) + feasible_obj_inner_bound_pose, 0.0, + object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) .position); } // create obj_polygon from inner/outer bound points tier4_autoware_utils::Polygon2d obj_poly; - for (const auto & bound_point : obj_inner_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); - } - std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); - for (const auto & bound_point : obj_outer_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); - } + const auto add_points_to_obj_poly = [&](const auto & bound_points) { + for (const auto & bound_point : bound_points) { + obj_poly.outer().push_back(tier4_autoware_utils::Point2d(bound_point.x, bound_point.y)); + } + }; + add_points_to_obj_poly(feasible_obj_inner_bound_points); + std::reverse(feasible_obj_outer_bound_points.begin(), feasible_obj_outer_bound_points.end()); + add_points_to_obj_poly(feasible_obj_outer_bound_points); boost::geometry::correct(obj_poly); return obj_poly; diff --git a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..16b4ffbc688ae --- /dev/null +++ b/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,123 @@ +// 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_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_dynamic_avoidance_module") + + "/config/dynamic_avoidance.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..86dc0ccb61330 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_external_request_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/manager.cpp + src/scene.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package() diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..dfcc4f61d8241 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp @@ -0,0 +1,54 @@ +// 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_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "behavior_path_lane_change_module/manager.hpp" +#include "route_handler/route_handler.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeRightModuleManager() + : LaneChangeModuleManager( + "external_request_lane_change_right", route_handler::Direction::RIGHT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; + +class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeLeftModuleManager() + + : LaneChangeModuleManager( + "external_request_lane_change_left", route_handler::Direction::LEFT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } + std::unique_ptr createNewSceneModuleInstance() override; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp similarity index 79% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp rename to planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp index d9c77d6db337e..c23d6f2f3996c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request.hpp +++ b/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" #include @@ -35,4 +35,4 @@ class ExternalRequestLaneChange : public NormalLaneChange }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_ +#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_external_request_lane_change_module/package.xml new file mode 100644 index 0000000000000..16216aa1f71e9 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/package.xml @@ -0,0 +1,39 @@ + + + + behavior_path_external_request_lane_change_module + 0.1.0 + The behavior_path_external_request_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_lane_change_module + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..f3cc686837c38 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..3cfe7aa51f0f1 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp @@ -0,0 +1,49 @@ +// 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_external_request_lane_change_module/manager.hpp" + +#include "behavior_path_external_request_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/interface.hpp" + +namespace behavior_path_planner +{ + +std::unique_ptr +ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +std::unique_ptr +ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, direction_)); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 92% rename from planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp rename to planning/behavior_path_external_request_lane_change_module/src/scene.cpp index 3a304a9b5bb53..913266bf79fa3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_external_request_lane_change_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" +#include "behavior_path_external_request_lane_change_module/scene.hpp" #include diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..434988cc3ab08 --- /dev/null +++ b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,128 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, { + planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", + }); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_goal_planner_module/README.md similarity index 96% rename from planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md rename to planning/behavior_path_goal_planner_module/README.md index 888d5d9ff9826..6bd2f8b9c79e4 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -91,7 +91,7 @@ Either one is activated when all conditions are met. If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. -![path_goal_refinement](../image/path_goal_refinement.drawio.svg) +![path_goal_refinement](./images/path_goal_refinement.drawio.svg) @@ -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** @@ -203,7 +204,7 @@ The lateral jerk is searched for among the predetermined minimum and maximum val 2. In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) 3. Combine this path with center line of road lane -![shift_parking](../image/shift_parking.drawio.svg) +![shift_parking](./images/shift_parking.drawio.svg) [shift_parking video](https://user-images.githubusercontent.com/39142679/178034101-4dc61a33-bc49-41a0-a9a8-755cce53cbc6.mp4) @@ -234,7 +235,7 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 Generate two forward arc paths. -![arc_forward_parking](../image/arc_forward_parking.drawio.svg) +![arc_forward_parking](./images/arc_forward_parking.drawio.svg) [arc_forward_parking video](https://user-images.githubusercontent.com/39142679/178034128-4754c401-8aff-4745-b69a-4a69ca29ce4b.mp4) @@ -251,7 +252,7 @@ Generate two forward arc paths. Generate two backward arc paths. -![arc_backward_parking](../image/arc_backward_parking.drawio.svg). +![arc_backward_parking](./images/arc_backward_parking.drawio.svg). [arc_backward_parking video](https://user-images.githubusercontent.com/39142679/178034280-4b6754fe-3981-4aee-b5e0-970f34563c6d.mp4) @@ -267,9 +268,9 @@ Generate two backward arc paths. ### freespace parking If the vehicle gets stuck with `lane_parking`, run `freespace_parking`. -To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_parking` to `true` +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../costmap_generator/README.md) to `false` and `enable_freespace_parking` to `true` -![pull_over_freespace_parking_flowchart](../image/pull_over_freespace_parking_flowchart.drawio.svg) +![pull_over_freespace_parking_flowchart](./images/pull_over_freespace_parking_flowchart.drawio.svg) Simultaneous execution with `avoidance_module` in the flowchart is under development. @@ -287,4 +288,4 @@ Simultaneous execution with `avoidance_module` in the flowchart is under develop | :----------------------- | :--- | :--- | :------------------------------------------------------------------------------------------------------------------- | :------------ | | enable_freespace_parking | [-] | bool | This flag enables freespace parking, which runs when the vehicle is stuck due to e.g. obstacles in the parking area. | true | -See [freespace_planner](../../freespace_planner/README.md) for other parameters. +See [freespace_planner](../freespace_planner/README.md) for other parameters. 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_planner/image/arc_backward_parking.drawio.svg b/planning/behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/arc_backward_parking.drawio.svg rename to planning/behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg diff --git a/planning/behavior_path_planner/image/arc_forward_parking.drawio.svg b/planning/behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/arc_forward_parking.drawio.svg rename to planning/behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg diff --git a/planning/behavior_path_planner/image/path_goal_refinement.drawio.svg b/planning/behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_goal_refinement.drawio.svg rename to planning/behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg diff --git a/planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg b/planning/behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/pull_over_freespace_parking_flowchart.drawio.svg rename to planning/behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg diff --git a/planning/behavior_path_planner/image/shift_parking.drawio.svg b/planning/behavior_path_goal_planner_module/images/shift_parking.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/shift_parking.drawio.svg rename to planning/behavior_path_goal_planner_module/images/shift_parking.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index c5ba04ba7f52e..6de1a726d4d4e 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include 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 7b5b9b1a68102..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 @@ -21,8 +21,8 @@ #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_goal_planner_module/goal_searcher.hpp" #include "behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_planner_common/utils/parking_departure/common_module_data.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" @@ -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,33 +482,22 @@ 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(); - // flag for the interface which do not support `allow_goal_modification` - // when the goal is in `road_shoulder`, always allow goal modification. - bool checkOriginalGoalIsInShoulder() const; - // steering factor void updateSteeringFactor( const std::array & pose, const std::array distance, const uint16_t type); 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 2d9e2c6cf3700..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 @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include @@ -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 cc8686f4a9fef..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 @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include @@ -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 791c35f3afca6..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 @@ -16,11 +16,10 @@ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include +#include "visualization_msgs/msg/detail/marker_array__struct.hpp" #include #include #include @@ -33,9 +32,7 @@ #include #include -namespace behavior_path_planner -{ -namespace goal_planner_utils +namespace behavior_path_planner::goal_planner_utils { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; @@ -48,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( @@ -68,7 +88,6 @@ MarkerArray createGoalCandidatesMarkerArray( MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); -} // namespace goal_planner_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::goal_planner_utils #endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index f028b8aff8b98..cbbe5f585dbe2 100644 --- a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -33,9 +33,8 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( BehaviorModuleOutput output = // use planner previous module reference path getPreviousModuleOutput(); - const PathWithLaneId smoothed_path = - modifyPathForSmoothGoalConnection(*(output.path), planner_data); - output.path = std::make_shared(smoothed_path); + const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(output.path, planner_data); + output.path = smoothed_path; output.reference_path = getPreviousModuleOutput().reference_path; return output; } diff --git a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp index c9af9ee369bd5..6d3eb09bf4352 100644 --- a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -15,8 +15,10 @@ #include "behavior_path_goal_planner_module/freespace_pull_over.hpp" #include "behavior_path_goal_planner_module/util.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_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 31c1d3ef77b0e..b7d1c240d032a 100644 --- a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -15,6 +15,7 @@ #include "behavior_path_goal_planner_module/geometric_pull_over.hpp" #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include #include 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 98f5664153bfd..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,7 @@ #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" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -61,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_); @@ -93,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_); @@ -136,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; } @@ -149,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 @@ -173,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); @@ -188,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); } @@ -198,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); } } @@ -213,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; } @@ -225,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; } @@ -282,7 +344,7 @@ void GoalPlannerModule::updateData() resetPathCandidate(); resetPathReference(); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); updateOccupancyGrid(); @@ -354,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) { @@ -384,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) { @@ -399,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 @@ -440,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); } @@ -585,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(); } @@ -603,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(); } @@ -743,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; } @@ -753,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 @@ -761,7 +830,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const // because it takes time for the trajectory to be reflected auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); keepStoppedWithCurrentPath(current_path); - output.path = std::make_shared(current_path); + output.path = current_path; } setModifiedGoal(output); @@ -773,51 +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 = std::make_shared(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 = std::make_shared(*stop_path); - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); - } else { - output.path = - std::make_shared(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) { @@ -826,7 +850,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); + output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -853,9 +877,9 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const { const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( - *output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); } @@ -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() @@ -942,10 +1038,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() BehaviorModuleOutput output{}; const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(); output.modified_goal = pull_over_output.modified_goal; - output.path = std::make_shared(generateStopPath()); + output.path = generateStopPath(); + output.reference_path = getPreviousModuleOutput().reference_path; const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); + output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info{}; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -956,6 +1053,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() return output; } + setDebugData(); + return output; } @@ -966,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(); @@ -1023,7 +1123,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updatePreviousData(output); + updatePreviousData(); return output; } @@ -1050,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 = 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) { @@ -1079,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(); } @@ -1153,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 @@ -1165,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. @@ -1176,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; @@ -1434,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)); @@ -1881,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 @@ -1894,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( @@ -1957,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 @@ -2015,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 @@ -2036,21 +2149,6 @@ void GoalPlannerModule::printParkingPositionError() const distance_from_real_shoulder); } -bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const -{ - const auto & route_handler = planner_data_->route_handler; - const Pose goal_pose = route_handler->getOriginalGoalPose(); - - const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); - - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); -} - bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const { return !isOnModifiedGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); 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 b087827a9c539..e2d12dd82cbe4 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -17,6 +17,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/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -169,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)) { @@ -263,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); @@ -312,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 a01f5680dcc13..8ad90d26f58f2 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -14,9 +14,13 @@ #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 #include #include +#include #include @@ -30,17 +34,15 @@ #include #include +namespace behavior_path_planner::goal_planner_utils +{ + using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; -namespace behavior_path_planner -{ -namespace goal_planner_utils -{ - 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,23 +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 isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_threshold) +{ + return calcLateralDeviationBetweenPaths(reference_path, target_path) < + lateral_deviation_threshold; +} + +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; + } + + // 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; } -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length) { - const Pose & goal_pose = route_handler->getGoalPose(); - const auto shoulder_lanes = route_handler->getShoulderLanelets(); + 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); - 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); + 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); } - return false; + 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 goal_planner_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..e5bce68ebfa44 --- /dev/null +++ b/planning/behavior_path_lane_change_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/interface.cpp + src/manager.cpp + src/scene.cpp + src/utils/markers.cpp + src/utils/utils.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + test/test_lane_change_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_lane_change_module/README.md similarity index 96% rename from planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md rename to planning/behavior_path_lane_change_module/README.md index ee74f700c4d42..bffa5201f2882 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -15,7 +15,7 @@ The Lane Change module is activated when lane change is needed and can be safely The lane change candidate path is divided into two phases: preparation and lane-changing. The following figure illustrates each phase of the lane change candidate path. -![lane-change-phases](../image/lane_change/lane_change-lane_change_phases.png) +![lane-change-phases](./images/lane_change-lane_change_phases.png) ### Preparation phase @@ -62,7 +62,7 @@ Note that when the `current_velocity` is lower than `minimum_lane_changing_veloc The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. -![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) +![path_samples](./images/lane_change-candidate_path_samples.png) Which path will be chosen will depend on validity and collision check. @@ -155,21 +155,21 @@ stop #### Candidate Path's Safety check -See [safety check utils explanation](../docs/behavior_path_planner_safety_check.md) +See [safety check utils explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) #### Objects selection and classification First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`. -![object lanes](../image/lane_change/lane_objects.drawio.svg) +![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../docs/behavior_path_planner_avoidance_design.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). ##### Collision check in prepare phase The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. -![enable collision check at prepare phase](../image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png) +![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. @@ -184,7 +184,7 @@ minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_ve The following figure illustrates when the lane is blocked in multiple lane changes cases. -![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) #### Stopping position when an object exists ahead @@ -195,25 +195,25 @@ The position to be stopped depends on the situation, such as when the lane chang Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_at_terminal_no_block](../image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg) +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) -![stop_at_terminal](../image/lane_change/lane_change-stop_at_terminal.drawio.svg) +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) ##### When the ego vehicle is not near the end of the lane change If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal_no_blocking_object](../image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. -![stop_not_at_terminal](../image/lane_change/lane_change-stop_not_at_terminal.drawio.svg) +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) ##### When the target lane is far away When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. -![stop_far_from_target_lane](../image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg) +![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) ### Lane Change When Stuck @@ -292,19 +292,19 @@ The function can be enabled by setting `enable_on_prepare_phase` to `true`. The following image illustrates the cancel process. -![cancel](../image/lane_change/cancel_and_abort/lane_change-cancel.png) +![cancel](./images/lane_change-cancel.png) #### Abort Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. -![abort](../image/lane_change/cancel_and_abort/lane_change-abort.png) +![abort](./images/lane_change-abort.png) #### Stop/Cruise The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. -![stop](../image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png) +![stop](./images/lane_change-cant_cancel_no_abort.png) ## Parameters @@ -451,11 +451,11 @@ Then add the marker in `rviz2`. -![debug](../image/lane_change/lane_change-debug-1.png) +![debug](./images/lane_change-debug-1.png) -![debug2](../image/lane_change/lane_change-debug-2.png) +![debug2](./images/lane_change-debug-2.png) -![debug3](../image/lane_change/lane_change-debug-3.png) +![debug3](./images/lane_change-debug-3.png) Available information diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/lane_change/lane_change.param.yaml rename to planning/behavior_path_lane_change_module/config/lane_change.param.yaml diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png b/planning/behavior_path_lane_change_module/images/lane_change-abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-abort.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png b/planning/behavior_path_lane_change_module/images/lane_change-cancel.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cancel.png rename to planning/behavior_path_lane_change_module/images/lane_change-cancel.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png b/planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png rename to planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png diff --git a/planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/cancel_and_abort/lane_change-cant_cancel_no_abort.png rename to planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-1.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-1.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-2.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-2.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png b/planning/behavior_path_lane_change_module/images/lane_change-debug-3.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png rename to planning/behavior_path_lane_change_module/images/lane_change-debug-3.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png b/planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-enable_collision_check_at_prepare_phase.png rename to planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png b/planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png rename to planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png b/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png rename to planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_at_terminal_no_block.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_far_from_target_lane.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png b/planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png rename to planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png diff --git a/planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_change.drawio.svg diff --git a/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg b/planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg rename to planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index b7cef007d6d9f..4e741a2b3db2c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" -#include "behavior_path_planner/scene_module/lane_change/external_request.hpp" -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/scene.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -68,11 +67,15 @@ class LaneChangeInterface : public SceneModuleInterface bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + bool isRootLaneletToBeUpdated() const override + { + return getCurrentStatus() == ModuleStatus::SUCCESS; + } void updateData() override; + void postProcess() override; + BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -117,15 +120,13 @@ class LaneChangeInterface : public SceneModuleInterface std::unique_ptr module_type_; - bool canTransitSuccessState() override { return false; } - - bool canTransitFailureState() override { return false; } + PathSafetyStatus post_process_safety_status_; - bool canTransitIdleToRunningState() override { return false; } + bool canTransitSuccessState() override; - void resetPathIfAbort(); + bool canTransitFailureState() override; - void resetLaneChangeModule(); + bool canTransitIdleToRunningState() override; void setObjectDebugVisualization() const; @@ -146,4 +147,4 @@ class LaneChangeInterface : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp similarity index 68% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp index 024c6685b28c4..31561b6210591 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "route_handler/route_handler.hpp" @@ -23,7 +23,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -46,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_; @@ -72,28 +73,6 @@ class LaneChangeLeftModuleManager : public LaneChangeModuleManager { } }; - -class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeRightModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_right", route_handler::Direction::RIGHT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; - -class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager -{ -public: - ExternalRequestLaneChangeLeftModuleManager() - : LaneChangeModuleManager( - "external_request_lane_change_left", route_handler::Direction::LEFT, - LaneChangeModuleType::EXTERNAL_REQUEST) - { - } -}; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp similarity index 94% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 61caf7b2b393f..f991ca0d849f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/base_class.hpp" +#include "behavior_path_lane_change_module/utils/base_class.hpp" #include #include @@ -71,7 +71,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isApprovedPathSafe() const override; - bool isRequiredStop(const bool is_object_coming_from_rear) const override; + bool isRequiredStop(const bool is_object_coming_from_rear) override; bool isNearEndOfCurrentLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, @@ -89,7 +89,7 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; - bool isLaneChangeRequired() const override; + bool isLaneChangeRequired() override; bool isStoppedAtRedTrafficLight() const override; @@ -180,4 +180,4 @@ class NormalLaneChange : public LaneChangeBase double stop_time_{0.0}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp similarity index 90% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index be49ec5b2e740..400d5505dc49f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -11,12 +11,12 @@ // 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__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -80,7 +80,7 @@ class LaneChangeBase virtual bool hasFinishedAbort() const = 0; - virtual bool isLaneChangeRequired() const = 0; + virtual bool isLaneChangeRequired() = 0; virtual bool isAbortState() const = 0; @@ -90,7 +90,7 @@ class LaneChangeBase virtual bool isEgoOnPreparePhase() const = 0; - virtual bool isRequiredStop(const bool is_object_coming_from_rear) const = 0; + virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; virtual PathSafetyStatus isApprovedPathSafe() const = 0; @@ -107,14 +107,13 @@ class LaneChangeBase virtual bool specialExpiredCheck() const { return false; } virtual void setPreviousModulePaths( - const std::shared_ptr & prev_module_reference_path, - const std::shared_ptr & prev_module_path) + const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path) { - if (prev_module_reference_path) { - prev_module_reference_path_ = *prev_module_reference_path; + if (!prev_module_reference_path.points.empty()) { + prev_module_reference_path_ = prev_module_reference_path; } - if (prev_module_path) { - prev_module_path_ = *prev_module_path; + if (!prev_module_path.points.empty()) { + prev_module_path_ = prev_module_path; } }; @@ -265,8 +264,8 @@ class LaneChangeBase mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; - rclcpp::Logger logger_ = rclcpp::get_logger("lane_change"); + rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp similarity index 70% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index 6f34bc79fe5f1..dd7760d31eaa7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -11,18 +11,61 @@ // 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__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ #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 + #include +#include #include namespace behavior_path_planner { +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; struct LaneChangeCancelParameters { @@ -33,6 +76,7 @@ struct LaneChangeCancelParameters double max_lateral_jerk{10.0}; double overhang_tolerance{0.0}; }; + struct LaneChangeParameters { // trajectory generation @@ -41,6 +85,15 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // lane change parameters + double backward_length_buffer_for_end_of_lane; + double backward_length_buffer_for_blocking_object; + double lane_changing_lateral_jerk{0.5}; + double minimum_lane_changing_velocity{5.6}; + double lane_change_prepare_duration{4.0}; + double lane_change_finish_judge_buffer{3.0}; + LateralAccelerationMap lane_change_lat_acc_map; + // parked vehicle double object_check_min_road_shoulder_width{0.5}; double object_shiftable_ratio_threshold{0.6}; @@ -158,4 +211,4 @@ struct PathSafetyStatus }; } // namespace behavior_path_planner::data::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index 96f0851ed33c0..d6deecd4f46fa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include + #include -#include #include namespace marker_utils::lane_change_markers @@ -38,4 +38,4 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); } // namespace marker_utils::lane_change_markers -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index c98f62c47860c..3af4f487a0fa0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_planner_common/turn_signal_decider.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -45,9 +45,9 @@ struct LaneChangeStatus std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; - bool is_valid_path{true}; + bool is_valid_path{false}; double start_distance{0.0}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp similarity index 85% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp rename to planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 12e6da00b2f36..67506326d92aa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "rclcpp/logger.hpp" #include @@ -54,17 +55,21 @@ using tier4_autoware_utils::Polygon2d; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, + const double length_to_intersection = 0.0); + double calcMaximumLaneChangeLength( - const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc); double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, - const BehaviorPathPlannerParameters & params); + const LaneChangeParameters & lane_change_parameters); double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); double calcLaneChangingAcceleration( const double initial_lane_changing_velocity, const double max_path_velocity, @@ -130,7 +135,7 @@ double getLateralShift(const LaneChangePath & path); bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & curent_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param, const Direction direction); + const LaneChangeParameters & lane_change_parameters, const Direction direction); lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -150,7 +155,8 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameters, const double resolution); + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution); bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, @@ -202,5 +208,17 @@ lanelet::ConstLanelets generateExpandedLanelets( const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, const double right_offset); +/** + * @brief Retrieves a logger instance for a specific lane change type. + * + * This function provides a specialized logger for different types of lane change. + * + * @param type A string representing the type of lane change operation. This could be + * a specific maneuver or condition related to lane changing, such as + * 'avoidance_by_lane_change', 'normal', 'external_request'. + * + * @return rclcpp::Logger The logger instance configured for the specified lane change type. + */ +rclcpp::Logger getLogger(const std::string & type); } // namespace behavior_path_planner::utils::lane_change -#endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/package.xml b/planning/behavior_path_lane_change_module/package.xml new file mode 100644 index 0000000000000..fb4bef9535a48 --- /dev/null +++ b/planning/behavior_path_lane_change_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_lane_change_module + 0.1.0 + The behavior_path_lane_change_module package + + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml similarity index 65% rename from planning/behavior_path_planner/plugins.xml rename to planning/behavior_path_lane_change_module/plugins.xml index fed2e1c9af3c3..a598bc8176938 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -1,8 +1,6 @@ - - - - + - + + diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp similarity index 75% rename from planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp rename to planning/behavior_path_lane_change_module/src/interface.cpp index cec9d5013d929..c8e1229fe5af5 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" +#include "behavior_path_lane_change_module/interface.hpp" -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/markers.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" @@ -26,7 +26,6 @@ #include #include #include -#include namespace behavior_path_planner { @@ -45,14 +44,12 @@ LaneChangeInterface::LaneChangeInterface( prev_approved_path_{std::make_unique()} { steering_factor_interface_ptr_ = std::make_unique(&node, name); + logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); } void LaneChangeInterface::processOnEntry() { waitApproval(); - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); } void LaneChangeInterface::processOnExit() @@ -64,7 +61,7 @@ void LaneChangeInterface::processOnExit() bool LaneChangeInterface::isExecutionRequested() const { - if (current_state_ == ModuleStatus::RUNNING) { + if (getCurrentStatus() == ModuleStatus::RUNNING) { return true; } @@ -76,119 +73,25 @@ bool LaneChangeInterface::isExecutionReady() const return module_type_->isSafe(); } -ModuleStatus LaneChangeInterface::updateState() +void LaneChangeInterface::updateData() { - auto log_warn_throttled = [&](const std::string & message) -> void { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, message); - }; - - if (module_type_->specialExpiredCheck()) { - log_warn_throttled("expired check."); - if (isWaitingApproval()) { - return ModuleStatus::SUCCESS; - } - } - - if (!isActivated() || isWaitingApproval()) { - log_warn_throttled("Is idling."); - return ModuleStatus::IDLE; - } - - if (!module_type_->isValidPath()) { - log_warn_throttled("Is invalid path."); - return ModuleStatus::SUCCESS; - } - - if (module_type_->isAbortState()) { - log_warn_throttled("Ego is in the process of aborting lane change."); - return module_type_->hasFinishedAbort() ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING; - } - - if (module_type_->hasFinishedLaneChange()) { - log_warn_throttled("Completed lane change."); - return ModuleStatus::SUCCESS; - } + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); - if (module_type_->isEgoOnPreparePhase() && module_type_->isStoppedAtRedTrafficLight()) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Ego stopped at traffic light. Canceling lane change"); - module_type_->toCancelState(); - return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; + if (isWaitingApproval()) { + module_type_->updateLaneChangeStatus(); } - - const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe(); - setObjectDebugVisualization(); - if (is_safe) { - log_warn_throttled("Lane change path is safe."); - module_type_->toNormalState(); - return ModuleStatus::RUNNING; - } - const auto change_state_if_stop_required = [&]() -> void { - if (module_type_->isRequiredStop(is_object_coming_from_rear)) { - module_type_->toStopState(); - } else { - module_type_->toNormalState(); - } - }; - - if (!module_type_->isCancelEnabled()) { - log_warn_throttled( - "Lane change path is unsafe but cancel was not enabled. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - if (!module_type_->isAbleToReturnCurrentLane()) { - log_warn_throttled("Lane change path is unsafe but cannot return. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - const auto & common_parameters = module_type_->getCommonParam(); - const auto threshold = common_parameters.backward_length_buffer_for_end_of_lane; - const auto status = module_type_->getLaneChangeStatus(); - if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) { - log_warn_throttled("Lane change path is unsafe but near end of lane. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) { - log_warn_throttled("Lane change path is unsafe. Cancel lane change."); - module_type_->toCancelState(); - return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; - } - - if (!module_type_->isAbortEnabled()) { - log_warn_throttled( - "Lane change path is unsafe but abort was not enabled. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - const auto found_abort_path = module_type_->calcAbortPath(); - if (!found_abort_path) { - log_warn_throttled( - "Lane change path is unsafe but not found abort path. Continue lane change."); - change_state_if_stop_required(); - return ModuleStatus::RUNNING; - } - - log_warn_throttled("Lane change path is unsafe. Abort lane change."); - module_type_->toAbortState(); - return ModuleStatus::RUNNING; + module_type_->updateSpecialData(); + module_type_->resetStopPose(); } -void LaneChangeInterface::updateData() +void LaneChangeInterface::postProcess() { - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateSpecialData(); - module_type_->resetStopPose(); + post_process_safety_status_ = module_type_->isApprovedPathSafe(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -200,11 +103,9 @@ BehaviorModuleOutput LaneChangeInterface::plan() return {}; } - module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); - module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); - path_reference_ = output.reference_path; - *prev_approved_path_ = *getPreviousModuleOutput().path; + path_reference_ = std::make_shared(output.reference_path); + *prev_approved_path_ = getPreviousModuleOutput().path; stop_pose_ = module_type_->getStopPose(); @@ -221,31 +122,23 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - *prev_approved_path_ = *getPreviousModuleOutput().path; + *prev_approved_path_ = getPreviousModuleOutput().path; module_type_->insertStopPoint( module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = std::make_shared(*prev_approved_path_); + out.path = *prev_approved_path_; out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); - setObjectDebugVisualization(); + out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); for (const auto & [uuid, data] : module_type_->getDebugData()) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - // change turn signal when the vehicle reaches at the end of the path for waiting lane change - out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); - - path_reference_ = getPreviousModuleOutput().reference_path; - + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { @@ -289,6 +182,136 @@ void LaneChangeInterface::setData(const std::shared_ptr & dat module_type_->setData(data); } +bool LaneChangeInterface::canTransitSuccessState() +{ + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + + if (module_type_->specialExpiredCheck() && isWaitingApproval()) { + log_debug_throttled("Run specialExpiredCheck."); + if (isWaitingApproval()) { + return true; + } + } + + if (!module_type_->isValidPath()) { + log_debug_throttled("Has no valid path."); + return true; + } + + if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { + log_debug_throttled("Abort process has completed."); + return true; + } + + if (module_type_->hasFinishedLaneChange()) { + module_type_->resetParameters(); + log_debug_throttled("Lane change process has completed."); + return true; + } + + log_debug_throttled("Lane changing process is ongoing"); + return false; +} + +bool LaneChangeInterface::canTransitFailureState() +{ + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + + log_debug_throttled(__func__); + + if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { + log_debug_throttled("Abort process has on going."); + return false; + } + + if (isWaitingApproval()) { + log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); + return false; + } + + if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { + if (module_type_->isStoppedAtRedTrafficLight()) { + log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); + module_type_->toCancelState(); + return true; + } + + if (post_process_safety_status_.is_safe) { + log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); + return false; + } + + if (module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("It's possible to return to current lane. Cancel lane change."); + return true; + } + } + + if (post_process_safety_status_.is_safe) { + log_debug_throttled("Can't transit to failure state. Ego is lane changing, and it's safe."); + return false; + } + + if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) { + log_debug_throttled("Module require stopping"); + } + + if (!module_type_->isCancelEnabled()) { + log_debug_throttled( + "Lane change path is unsafe but cancel was not enabled. Continue lane change."); + return false; + } + + if (!module_type_->isAbortEnabled()) { + log_debug_throttled( + "Lane change path is unsafe but abort was not enabled. Continue lane change."); + return false; + } + + if (!module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("It's is not possible to return to original lane. Continue lane change."); + return false; + } + + const auto found_abort_path = module_type_->calcAbortPath(); + if (!found_abort_path) { + log_debug_throttled( + "Lane change path is unsafe but abort path not found. Continue lane change."); + return false; + } + + log_debug_throttled("Lane change path is unsafe. Abort lane change."); + module_type_->toAbortState(); + return false; +} + +bool LaneChangeInterface::canTransitIdleToRunningState() +{ + setObjectDebugVisualization(); + + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + + log_debug_throttled(__func__); + + if (!isActivated() || isWaitingApproval()) { + if (module_type_->specialRequiredCheck()) { + return true; + } + log_debug_throttled("Module is idling."); + return false; + } + + log_debug_throttled("Can lane change safely. Executing lane change."); + module_type_->toNormalState(); + return true; +} + void LaneChangeInterface::setObjectDebugVisualization() const { debug_marker_.markers.clear(); @@ -337,7 +360,7 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() return marker; } - if (isWaitingApproval() || current_state_ != ModuleStatus::RUNNING) { + if (isWaitingApproval() || getCurrentStatus() != ModuleStatus::RUNNING) { return marker; } const auto & start_pose = module_type_->getLaneChangePath().info.lane_changing_start; @@ -368,9 +391,9 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); const auto start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_position, status.lane_change_path.info.shift_line.start.position); + output.path.points, current_position, status.lane_change_path.info.shift_line.start.position); const auto finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_position, status.lane_change_path.info.shift_line.end.position); + output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); steering_factor_interface_ptr_->updateSteeringFactor( {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, @@ -432,8 +455,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & common_parameter = module_type_->getCommonParam(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameter, shift_intervals, common_parameter.backward_length_buffer_for_end_of_lane); + const double next_lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; const double & base_to_front = common_parameter.base_link2front; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp similarity index 83% rename from planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp rename to planning/behavior_path_lane_change_module/src/manager.cpp index ed40476723dba..0a56c17d89fd0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_lane_change_module/manager.hpp" +#include "behavior_path_lane_change_module/interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -28,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{}; @@ -141,6 +146,49 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + // lane change parameters + const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.backward_length_buffer_for_end_of_lane = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); + p.backward_length_buffer_for_blocking_object = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + p.lane_changing_lateral_jerk = + getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); + p.lane_change_prepare_duration = + getOrDeclareParameter(*node, parameter("prepare_duration")); + p.minimum_lane_changing_velocity = + getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); + p.minimum_lane_changing_velocity = + std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); + p.lane_change_finish_judge_buffer = + getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); + + if (p.backward_length_buffer_for_end_of_lane < 1.0) { + RCLCPP_WARN_STREAM( + node->get_logger().get_child(name()), + "Lane change buffer must be more than 1 meter. Modifying the buffer."); + } + + // lateral acceleration map for lane change + const auto lateral_acc_velocity = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.min_values")); + const auto max_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.max_values")); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR( + node->get_logger().get_child(name()), + "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.lane_change_lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + // target object { const std::string ns = "lane_change.target_object."; @@ -219,16 +267,10 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() { - if (type_ == LaneChangeModuleType::NORMAL) { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); - } return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_, - std::make_unique(parameters_, direction_)); + std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) @@ -255,9 +297,3 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( behavior_path_planner::LaneChangeLeftModuleManager, behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp similarity index 93% rename from planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp rename to planning/behavior_path_lane_change_module/src/scene.cpp index 423ffc48dd6cd..f5100f16129c2 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_lane_change_module/scene.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -115,17 +115,17 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } -bool NormalLaneChange::isLaneChangeRequired() const +bool NormalLaneChange::isLaneChangeRequired() { - const auto current_lanes = getCurrentLanes(); + status_.current_lanes = getCurrentLanes(); - if (current_lanes.empty()) { + if (status_.current_lanes.empty()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); - return !target_lanes.empty(); + return !status_.target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -145,38 +145,38 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() BehaviorModuleOutput output; if (isAbortState() && abort_path_) { - output.path = std::make_shared(abort_path_->path); - output.reference_path = std::make_shared(prev_module_reference_path_); + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; output.turn_signal_info = prev_turn_signal_info_; - insertStopPoint(status_.current_lanes, *output.path); + insertStopPoint(status_.current_lanes, output.path); } else { - output.path = std::make_shared(getLaneChangePath().path); + output.path = getLaneChangePath().path; const auto found_extended_path = extendPath(); if (found_extended_path) { - *output.path = utils::combinePath(*output.path, *found_extended_path); + output.path = utils::combinePath(output.path, *found_extended_path); } - output.reference_path = std::make_shared(getReferencePath()); + output.reference_path = getReferencePath(); output.turn_signal_info = updateOutputTurnSignal(); if (isStopState()) { const auto current_velocity = getEgoVelocity(); const auto current_dist = calcSignedArcLength( - output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); + output.path.points, output.path.points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); setStopPose(stop_point.point.pose); } else { - insertStopPoint(status_.target_lanes, *output.path); + insertStopPoint(status_.target_lanes, output.path); } } extendOutputDrivableArea(output); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( - *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -189,7 +189,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *getRouteHandler(), status_.current_lanes, status_.target_lanes); - const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); @@ -215,9 +215,8 @@ void NormalLaneChange::insertStopPoint( } const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, - 0.0); + const double lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -233,7 +232,7 @@ void NormalLaneChange::insertStopPoint( distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); } - const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; @@ -294,15 +293,14 @@ void NormalLaneChange::insertStopPoint( if (distance_to_ego_lane_obj < distance_to_terminal) { // consider rss distance when the LC need to avoid obstacles const auto rss_dist = calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); - const double lane_change_buffer_for_blocking_object = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, - getCommonParam().backward_length_buffer_for_blocking_object, 0.0); + const double lane_change_buffer_for_blocking_object = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - - getCommonParam().backward_length_buffer_for_blocking_object - rss_dist - + lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - getCommonParam().base_link2front; // If the target lane in the lane change section is blocked by a stationary obstacle, there @@ -422,8 +420,15 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; + status_ = {}; object_debug_.clear(); + object_debug_after_approval_.clear(); + debug_filtered_objects_.current_lane.clear(); + debug_filtered_objects_.target_lane.clear(); + debug_filtered_objects_.other_lane.clear(); + debug_valid_path_.clear(); + RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() @@ -498,9 +503,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & current_pose = getEgoPose(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const auto lane_change_buffer = utils::calcMinimumLaneChangeLength( - planner_data_->parameters, shift_intervals, - getCommonParam().backward_length_buffer_for_end_of_lane); + const auto lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -519,7 +523,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; const double dist_to_lane_change_end = utils::getSignedDistance( current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); - double finish_judge_buffer = planner_data_->parameters.lane_change_finish_judge_buffer; + double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; // If ego velocity is low, relax finish judge buffer const double ego_velocity = getEgoVelocity(); @@ -560,7 +564,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const planner_data_->parameters.ego_nearest_yaw_threshold); const double ego_velocity = - std::max(getEgoVelocity(), planner_data_->parameters.minimum_lane_changing_velocity); + std::max(getEgoVelocity(), lane_change_parameters_->minimum_lane_changing_velocity); const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; double dist = 0.0; @@ -664,10 +668,10 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps; // calculate minimum and maximum acceleration - const auto min_acc = - utils::lane_change::calcMinimumAcceleration(getEgoVelocity(), vehicle_min_acc, p); + const auto min_acc = utils::lane_change::calcMinimumAcceleration( + getEgoVelocity(), vehicle_min_acc, *lane_change_parameters_); const auto max_acc = utils::lane_change::calcMaximumAcceleration( - getEgoVelocity(), max_path_velocity, vehicle_max_acc, p); + getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_); return {min_acc, max_acc}; } @@ -678,8 +682,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength( const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); return utils::lane_change::calcMaximumLaneChangeLength( - std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(), - shift_intervals, max_acc); + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()), + *lane_change_parameters_, shift_intervals, max_acc); } std::vector NormalLaneChange::sampleLongitudinalAccValues( @@ -746,7 +750,6 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( std::vector NormalLaneChange::calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { - const auto & common_parameters = planner_data_->parameters; const auto base_link2front = planner_data_->parameters.base_link2front; const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; @@ -754,7 +757,7 @@ std::vector NormalLaneChange::calcPrepareDuration( std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = common_parameters.lane_change_prepare_duration; duration >= 0.0; + for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { @@ -1001,12 +1004,7 @@ PathWithLaneId NormalLaneChange::getTargetSegment( std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); }); - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("getTargetSegment"), - "start: %f, end: %f", s_start, s_end); + RCLCPP_DEBUG(logger_, "in %s start: %f, end: %f", __func__, s_start, s_end); PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end); for (auto & point : target_segment.points) { @@ -1024,12 +1022,11 @@ bool NormalLaneChange::hasEnoughLength( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - const auto & common_parameters = planner_data_->parameters; const double lane_change_length = path.info.length.sum(); const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); - double minimum_lane_change_length_to_preferred_lane = utils::calcMinimumLaneChangeLength( - common_parameters, shift_intervals, common_parameters.backward_length_buffer_for_end_of_lane); + double minimum_lane_change_length_to_preferred_lane = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1125,7 +1122,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto backward_path_length = common_parameters.backward_path_length; const auto forward_path_length = common_parameters.forward_path_length; - const auto minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; // get velocity @@ -1137,12 +1135,12 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), - common_parameters.backward_length_buffer_for_end_of_lane); - const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()), - common_parameters.backward_length_buffer_for_end_of_lane); + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); @@ -1219,7 +1217,7 @@ bool NormalLaneChange::getLaneChangePaths( // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); + lane_change_parameters_->lane_change_lat_acc_map.find(initial_lane_changing_velocity); const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; @@ -1238,7 +1236,7 @@ bool NormalLaneChange::getLaneChangePaths( }; const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); const double longitudinal_acc_on_lane_changing = utils::lane_change::calcLaneChangingAcceleration( initial_lane_changing_velocity, max_path_velocity, lane_changing_time, @@ -1264,8 +1262,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); const double backward_buffer = - num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; + num == 0 ? 0.0 : lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = + lane_change_parameters_->lane_change_finish_judge_buffer; if ( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > @@ -1534,11 +1533,17 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const return true; } -bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) const +bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) { - const auto threshold = planner_data_->parameters.backward_length_buffer_for_end_of_lane; - return isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && - isAbleToStopSafely() && is_object_coming_from_rear; + const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; + if ( + isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && + isAbleToStopSafely() && is_object_coming_from_rear) { + current_lane_change_state_ = LaneChangeStates::Stop; + return true; + } + current_lane_change_state_ = LaneChangeStates::Normal; + return false; } bool NormalLaneChange::calcAbortPath() @@ -1546,7 +1551,7 @@ bool NormalLaneChange::calcAbortPath() const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = - std::max(common_param.minimum_lane_changing_velocity, getEgoVelocity()); + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; const auto reference_lanelets = selected_path.info.current_lanes; @@ -1557,8 +1562,8 @@ bool NormalLaneChange::calcAbortPath() const auto direction = getDirection(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( selected_path.info.current_lanes.back(), direction); - const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( - common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1605,8 +1610,8 @@ bool NormalLaneChange::calcAbortPath() } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param, - direction)) { + *route_handler, reference_lanelets, current_pose, abort_return_dist, + *lane_change_parameters_, direction)) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -1629,7 +1634,8 @@ bool NormalLaneChange::calcAbortPath() const auto lateral_jerk = behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); - const auto lateral_acc_range = common_param.lane_change_lat_acc_map.find(current_velocity); + const auto lateral_acc_range = + lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); const double & max_lateral_acc = lateral_acc_range.second; path_shifter.setLateralAccelerationLimit(max_lateral_acc); @@ -1662,7 +1668,7 @@ bool NormalLaneChange::calcAbortPath() PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); ref.points.back().point.longitudinal_velocity_mps = std::min( ref.points.back().point.longitudinal_velocity_mps, - static_cast(common_param.minimum_lane_changing_velocity)); + static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); return ref; }); @@ -1707,7 +1713,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const double & time_resolution = lane_change_parameters_->prediction_time_resolution; const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, common_parameters, time_resolution); + lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_, + time_resolution); const auto debug_predicted_path = utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); @@ -1818,10 +1825,9 @@ bool NormalLaneChange::isVehicleStuck( : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, - 0.0); - const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); + const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { return true; @@ -1844,7 +1850,7 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); const auto rss_dist = calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp similarity index 98% rename from planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp rename to planning/behavior_path_lane_change_module/src/utils/markers.cpp index c2a640f989b50..719acba405a68 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -16,7 +16,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp similarity index 92% rename from planning/behavior_path_planner/src/utils/lane_change/utils.cpp rename to planning/behavior_path_lane_change_module/src/utils/utils.cpp index 66182454ecf85..df73990b770a3 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -67,17 +67,43 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, + const double length_to_intersection) +{ + if (shift_intervals.empty()) { + return 0.0; + } + + const double & vel = lane_change_parameters.minimum_lane_changing_velocity; + const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); + const double & max_lateral_acc = lat_acc.second; + const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const double & backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; + + double accumulated_length = length_to_intersection; + for (const auto & shift_interval : shift_intervals) { + const double t = + PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); + accumulated_length += vel * t + finish_judge_buffer; + } + accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); + + return accumulated_length; +} + double calcMaximumLaneChangeLength( - const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc) { if (shift_intervals.empty()) { return 0.0; } - const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - const double & lateral_jerk = common_param.lane_changing_lateral_jerk; - const double & t_prepare = common_param.lane_change_prepare_duration; + const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const double & t_prepare = lane_change_parameters.lane_change_prepare_duration; double vel = current_velocity; double accumulated_length = 0.0; @@ -87,7 +113,7 @@ double calcMaximumLaneChangeLength( vel = vel + max_acc * t_prepare; // lane changing section - const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); + const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); const double t_lane_changing = PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, lat_acc.second); const double lane_changing_length = @@ -97,26 +123,26 @@ double calcMaximumLaneChangeLength( vel = vel + max_acc * t_lane_changing; } accumulated_length += - common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); + lane_change_parameters.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); return accumulated_length; } double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, - const BehaviorPathPlannerParameters & params) + const LaneChangeParameters & lane_change_parameters) { - const double min_lane_changing_velocity = params.minimum_lane_changing_velocity; - const double prepare_duration = params.lane_change_prepare_duration; + const double min_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); } double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) { - const double prepare_duration = params.lane_change_prepare_duration; + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; const double acc = (current_max_velocity - current_velocity) / prepare_duration; return std::clamp(acc, 0.0, max_longitudinal_acc); } @@ -218,7 +244,9 @@ lanelet::ConstLanelets getTargetNeighborLanes( for (const auto & current_lane : current_lanes) { if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { neighbor_lanes.push_back(current_lane); } } else { @@ -601,12 +629,12 @@ double getLateralShift(const LaneChangePath & path) bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param, const Direction direction) + const LaneChangeParameters & lane_change_parameters, const Direction direction) { const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); - const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( - common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); + const double minimum_lane_change_length = + calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -756,7 +784,9 @@ std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { return route_handler.getLaneChangeTarget(current_lanes, direction); } @@ -765,7 +795,8 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const BehaviorPathPlannerParameters & common_parameters, const double resolution) + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution) { if (lane_change_path.path.points.empty()) { return {}; @@ -776,7 +807,8 @@ std::vector convertToPredictedPath( const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; const auto duration = lane_change_path.info.duration.sum(); const auto prepare_time = lane_change_path.info.duration.prepare; - const auto & minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity; + const auto & minimum_lane_changing_velocity = + lane_change_parameters.minimum_lane_changing_velocity; const auto nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, @@ -1065,7 +1097,8 @@ std::optional createPolygon( } ExtendedPredictedObject transform( - const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, + const PredictedObject & object, + [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object; @@ -1078,7 +1111,7 @@ ExtendedPredictedObject transform( const auto & time_resolution = lane_change_parameters.prediction_time_resolution; const auto & check_at_prepare_phase = lane_change_parameters.enable_prepare_segment_collision_check; - const auto & prepare_duration = common_parameters.lane_change_prepare_duration; + const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; @@ -1130,4 +1163,9 @@ lanelet::ConstLanelets generateExpandedLanelets( const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); } + +rclcpp::Logger getLogger(const std::string & type) +{ + return rclcpp::get_logger("lane_change").get_child(type); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..82f721411d5a4 --- /dev/null +++ b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp similarity index 96% rename from planning/behavior_path_planner/test/test_lane_change_utils.cpp rename to planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 3268bd7ec468e..1f90114df8045 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include #include @@ -41,7 +41,7 @@ TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) TEST(BehaviorPathPlanningLaneChangeUtilsTest, TESTLateralAccelerationMap) { - LateralAccelerationMap lat_acc_map; + behavior_path_planner::LateralAccelerationMap lat_acc_map; lat_acc_map.add(0.0, 0.2, 0.315); lat_acc_map.add(3.0, 0.2, 0.315); lat_acc_map.add(5.0, 0.2, 0.315); diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 75f9e7f7af187..54b4bf122da00 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,23 +7,9 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) - ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp - src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp - src/scene_module/dynamic_avoidance/manager.cpp - src/scene_module/side_shift/side_shift_module.cpp - src/scene_module/side_shift/manager.cpp - src/scene_module/lane_change/interface.cpp - src/scene_module/lane_change/normal.cpp - src/scene_module/lane_change/external_request.cpp - src/scene_module/lane_change/manager.cpp - src/utils/lane_change/utils.cpp - src/utils/side_shift/util.cpp - src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_utils/lane_change/debug.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC @@ -49,14 +35,6 @@ if(BUILD_TESTING) ${PROJECT_NAME}_lib ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module - test/test_lane_change_utils.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module - ${PROJECT_NAME}_lib - ) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface test/test_${PROJECT_NAME}_node_interface.cpp ) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 884767315ac12..03f8c8d2a5696 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -24,17 +24,17 @@ Essentially, the module has three primary responsibilities: Behavior Path Planner has following scene modules -| Name | Description | Details | -| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | -| Dynamic Avoidance | WIP | LINK | -| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_by_lane_change_design.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | -| External Lane Change | WIP | LINK | -| Start Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_goal_planner_design.md) | -| Goal Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_start_planner_design.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | +| Name | Description | Details | +| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_module/README.md) | +| Dynamic Avoidance | WIP | [LINK](../behavior_path_dynamic_avoidance_module/README.md) | +| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | +| External Lane Change | WIP | LINK | +| Start Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | +| Goal Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | !!! Note @@ -139,7 +139,6 @@ Enabling and disabling the modules in the behavior path planner is primarily man The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: - `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. -- `use_experimental_lane_change_function`: Set to `true` to enable experimental features in the lane change module. !!! note @@ -169,7 +168,7 @@ The shifted path generation logic enables the behavior path planner to dynamical !!! note - If you're a math lover, refer to [Path Generation Design](./docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. + If you're a math lover, refer to [Path Generation Design](../behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. ## Collision Assessment / Safety check @@ -186,7 +185,7 @@ However, the module does have a limitation concerning the yaw angle of each poin !!! note - For further reading on the collision assessment method, please refer to [Safety check utils](./docs/behavior_path_planner_safety_check.md) + For further reading on the collision assessment method, please refer to [Safety check utils](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) ## Generating Drivable Area @@ -209,7 +208,7 @@ Static drivable area expansion operates under assumptions about the correct arra !!! note - Further details can is provided in [Drivable Area Design](./docs/behavior_path_planner_drivable_area_design.md). + Further details can is provided in [Drivable Area Design](../behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). ### Dynamic Drivable Area Logic @@ -229,7 +228,7 @@ The `TurnIndicatorsCommand` message structure has a command field that can take !!! note - For more in-depth information, refer to [Turn Signal Design](./docs/behavior_path_planner_turn_signal_design.md) document. + For more in-depth information, refer to [Turn Signal Design](../behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md) document. ## Rerouting diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index b4d323de45cde..0c5dbc082c9b9 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,8 +25,6 @@ input_path_interval: 2.0 output_path_interval: 2.0 - visualize_maximum_drivable_area: true - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index c0b6f259c7726..bb897db393b65 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -13,7 +13,7 @@ smoothing: curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length - extra_arc_length: 2.0 # [m] extra arc length where an expansion distance is initially applied + arc_length_range: 2.0 # [m] arc length range where an expansion distance is initially applied ego: extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase extra_front_overhang: 0.5 # [m] extra length to add to the front overhang 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/docs/behavior_path_planner_drivable_area_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md deleted file mode 100644 index 056e41781e262..0000000000000 --- a/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md +++ /dev/null @@ -1,143 +0,0 @@ -# Drivable Area design - -Drivable Area represents the area where ego vehicle can pass. - -## Purpose / Role - -In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function. - -## Assumption - -Our drivable area has several assumptions. - -- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes. - -- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections). - -- Both left and right bounds should cover the front of the path and the end of the path. - -## Limitations - -Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative. - -## Parameters for drivable area generation - -| Name | Unit | Type | Description | Default value | -| :------------------------------------------- | :--- | :----------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------ | -| enabled | [-] | boolean | whether to dynamically the drivable area using the ego footprint | false | -| ego.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.0 | -| ego.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.0 | -| ego.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.0 | -| ego.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.0 | -| dynamic_objects.avoid | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true | -| dynamic_objects.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.5 | -| dynamic_objects.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | -| dynamic_objects.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.5 | -| dynamic_objects.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | -| max_distance | [m] | double | maximum distance by which the drivable area can be expended. A value of 0.0 means no maximum distance. | 0.0 | -| expansion.method | [-] | string | method to use for the expansion: "polygon" will expand the drivable area around the ego footprint polygons; "lanelet" will expand to the whole lanelets overlapped by the ego footprints | "polygon" | -| expansion.max_arc_path_length | [m] | double | maximum length along the path where the ego footprint is projected | 50.0 | -| expansion.extra_arc_length | [m] | double | extra arc length (relative to the path) around ego footprints where the drivable area is expanded | 0.5 | -| expansion.avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | [guard_rail, road_border] | -| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 | -| avoid_linestring.compensate.enable | [-] | bool | if true, when the expansion is blocked by a linestring on one side of the path, we try to compensate and expand on the other side | true | -| avoid_linestring.compensate.extra_distance | [m] | double | extra distance added to the expansion when compensating | 3.0 | - -The following parameters are defined for each module. Please refer to the `config/drivable_area_expansion.yaml` file. - -| Name | Unit | Type | Description | Default value | -| :------------------------------- | :--- | :----- | :------------------------------------------------------------------------- | :------------ | -| drivable_area_right_bound_offset | [m] | double | right offset length to expand drivable area | 5.0 | -| drivable_area_left_bound_offset | [m] | double | left offset length to expand drivable area | 5.0 | -| drivable_area_types_to_skip | [-] | string | linestring types (as defined in the lanelet map) that will not be expanded | road_border | - -## Inner-workings / Algorithms - -This section gives details of the generation of the drivable area (`left_bound` and `right_bound`). - -### Drivable Lanes Generation - -Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Goal Planner`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. - -```cpp -struct DrivalbleLanes -{ - lanelet::ConstLanelet right_lanelet; // right most lane - lanelet::ConstLanelet left_lanelet; // left most lane - lanelet::ConstLanelets middle_lanelets; // middle lanes -}; -``` - -The image of the sorted drivable lanes is depicted in the following picture. - -![sorted_lanes](../image/drivable_area/sorted_lanes.drawio.svg) - -Note that, the order of drivable lanes become - -```cpp -drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5} -``` - -### Drivable Area Generation - -In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as - -```cpp -std::vector left_bound; -std::vector right_bound; -``` - -and each point of right bound and left bound has a position in the absolute coordinate system. - -![drivable_lines](../image/drivable_area/drivable_lines.drawio.svg) - -### Drivable Area Expansion - -#### Static Expansion - -Each module can statically expand the left and right bounds of the target lanes by the parameter defined values. -This enables large vehicles to pass narrow curve. The image of this process can be described as - -![expanded_lanes](../image/drivable_area/expanded_lanes.drawio.svg) - -Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane. - -#### Dynamic Expansion - -The drivable area can also be expanded dynamically by considering the ego vehicle footprint projected on each path point. -This expansion can be summarized with the following steps: - -1. Build the ego path footprint. -2. Build the dynamic objects' predicted footprints (optional). -3. Build "uncrossable" lines. -4. Remove the footprints from step 2 and the lines from step 3 from the ego path footprint from step 1. -5. Expand the drivable area with the result of step 4. - -| | | -| :------------------------------- | :--------------------------------------------------------------------------------------------------- | -| Inputs | ![drivable_area_expansion_inputs](../image/drivable_area/drivable_area_expansion_inputs.png) | -| Footprints and uncrossable lines | ![drivable_area_expansion_footprints](../image/drivable_area/drivable_area_expansion_footprints.png) | -| Expanded drivable area | ![drivable_area_expansion_result](../image/drivable_area/drivable_area_expansion_result.png) | - -Please note that the dynamic expansion can only increase the size of the drivable area and cannot remove any part from the original drivable area. - -### Visualizing maximum drivable area (Debug) - -Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. - -For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user. - -To debug the issue, the maximum drivable area boundary can be visualized. - -![drivable_area_boundary_marker1](../image/drivable_area/drivable_area_boundary_marker_example1.png) - -![drivable_area_boundary_marker2](../image/drivable_area/drivable_area_boundary_marker_example2.png) - -The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area` - -#### Expansion with hatched road markings area - -If the hatched road markings area is defined in the lanelet map, the area can be used as a drivable area. -Since the area is expressed as a polygon format of Lanelet2, several steps are required for correct expansion. - -![hatched_road_markings_drivable_area_expansion](../image/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg) diff --git a/planning/behavior_path_planner/image/avoid_fig_multi_case.png b/planning/behavior_path_planner/image/avoid_fig_multi_case.png deleted file mode 100644 index 014806fa7a4b6..0000000000000 Binary files a/planning/behavior_path_planner/image/avoid_fig_multi_case.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/avoid_fig_single_case.png b/planning/behavior_path_planner/image/avoid_fig_single_case.png deleted file mode 100644 index 7b50a286dca99..0000000000000 Binary files a/planning/behavior_path_planner/image/avoid_fig_single_case.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_footprints.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_footprints.png deleted file mode 100644 index 0ec8bf7f5cf88..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_footprints.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_inputs.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_inputs.png deleted file mode 100644 index d2ba94a5b21aa..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_inputs.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_result.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_result.png deleted file mode 100644 index 373cf4899086d..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area/drivable_area_expansion_result.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/drivable_area_plugin.png b/planning/behavior_path_planner/image/drivable_area_plugin.png deleted file mode 100644 index d63e7b71934c3..0000000000000 Binary files a/planning/behavior_path_planner/image/drivable_area_plugin.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/lane_change_fig1.png b/planning/behavior_path_planner/image/lane_change_fig1.png deleted file mode 100644 index 74fbf25f86394..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change_fig1.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/lane_change_fig2.png b/planning/behavior_path_planner/image/lane_change_fig2.png deleted file mode 100644 index 5a87e43733520..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change_fig2.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/lane_change_fig3.png b/planning/behavior_path_planner/image/lane_change_fig3.png deleted file mode 100644 index 9e1791a05d098..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change_fig3.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/minimum_lane_change_distance.png b/planning/behavior_path_planner/image/minimum_lane_change_distance.png deleted file mode 100644 index cdde3be0eff38..0000000000000 Binary files a/planning/behavior_path_planner/image/minimum_lane_change_distance.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/path_shifter_old.png b/planning/behavior_path_planner/image/path_shifter_old.png deleted file mode 100644 index 91542fdd0d042..0000000000000 Binary files a/planning/behavior_path_planner/image/path_shifter_old.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/shift_length_computation.png b/planning/behavior_path_planner/image/shift_length_computation.png deleted file mode 100644 index 5c6f364ececd0..0000000000000 Binary files a/planning/behavior_path_planner/image/shift_length_computation.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/side_shift_fig1.png b/planning/behavior_path_planner/image/side_shift_fig1.png deleted file mode 100644 index e851e84ffb555..0000000000000 Binary files a/planning/behavior_path_planner/image/side_shift_fig1.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/start_from_road_lane.drawio.svg b/planning/behavior_path_planner/image/start_from_road_lane.drawio.svg deleted file mode 100644 index 7a89cc2183acf..0000000000000 --- a/planning/behavior_path_planner/image/start_from_road_lane.drawio.svg +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index c47dc70213d1a..5d40f2a8614ed 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -166,7 +166,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node const BehaviorModuleOutput & output); // debug - rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; 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 7ceafb6f428de..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, @@ -286,6 +287,8 @@ class PlannerManager module_ptr->publishRTCStatus(); + module_ptr->publishSteeringFactor(); + module_ptr->publishObjectsOfInterestMarker(); processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true); @@ -433,7 +436,16 @@ class PlannerManager const std::vector & request_modules, const std::shared_ptr & data, const BehaviorModuleOutput & previous_module_output); - std::string getNames(const std::vector & modules) const; + /** + * @brief run keep last approved modules + * @param planner data. + * @param previous module output. + * @return planning result. + */ + BehaviorModuleOutput runKeepLastModules( + const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) const; + + static std::string getNames(const std::vector & modules); std::optional root_lanelet_{std::nullopt}; @@ -445,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/package.xml b/planning/behavior_path_planner/package.xml index 0ca7badd0b3f4..2bc73325f96d5 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -56,7 +56,6 @@ pluginlib rclcpp rclcpp_components - route_handler sensor_msgs signal_processing tf2 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 907c5226c3908..3eb5eca5954fb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include #include @@ -73,11 +74,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); - if (planner_data_->parameters.visualize_maximum_drivable_area) { - debug_maximum_drivable_area_publisher_ = - create_publisher("~/maximum_drivable_area", 1); - } - debug_turn_signal_info_publisher_ = create_publisher("~/debug/turn_signal_info", 1); bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -135,6 +131,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } planner_manager_->launchScenePlugin(*this, name); } @@ -238,39 +238,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.min_acc = declare_parameter("normal.min_acc"); p.max_acc = declare_parameter("normal.max_acc"); - // lane change parameters - p.backward_length_buffer_for_end_of_lane = - declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); - p.backward_length_buffer_for_blocking_object = - declare_parameter("lane_change.backward_length_buffer_for_blocking_object"); - p.lane_changing_lateral_jerk = - declare_parameter("lane_change.lane_changing_lateral_jerk"); - p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); - p.minimum_lane_changing_velocity = - declare_parameter("lane_change.minimum_lane_changing_velocity"); - p.minimum_lane_changing_velocity = - std::min(p.minimum_lane_changing_velocity, p.max_acc * p.lane_change_prepare_duration); - p.lane_change_finish_judge_buffer = - declare_parameter("lane_change.lane_change_finish_judge_buffer"); - - // lateral acceleration map for lane change - const auto lateral_acc_velocity = - declare_parameter>("lane_change.lateral_acceleration.velocity"); - const auto min_lateral_acc = - declare_parameter>("lane_change.lateral_acceleration.min_values"); - const auto max_lateral_acc = - declare_parameter>("lane_change.lateral_acceleration.max_values"); - if ( - lateral_acc_velocity.size() != min_lateral_acc.size() || - lateral_acc_velocity.size() != max_lateral_acc.size()) { - RCLCPP_ERROR(get_logger(), "Lane change lateral acceleration map has invalid size."); - exit(EXIT_FAILURE); - } - for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { - p.lane_change_lat_acc_map.add( - lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); - } - p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over"); p.backward_length_buffer_for_end_of_pull_out = @@ -293,14 +260,9 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.enable_cog_on_centerline = declare_parameter("enable_cog_on_centerline"); p.input_path_interval = declare_parameter("input_path_interval"); p.output_path_interval = declare_parameter("output_path_interval"); - p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area"); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - if (p.backward_length_buffer_for_end_of_lane < 1.0) { - RCLCPP_WARN_STREAM( - get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); - } return p; } @@ -480,12 +442,6 @@ void BehaviorPathPlannerNode::run() planner_data_->prev_route_id = planner_data_->route_handler->getRouteUuid(); - if (planner_data_->parameters.visualize_maximum_drivable_area) { - const auto maximum_drivable_area = marker_utils::createFurthestLineStringMarkerArray( - utils::getMaximumDrivableArea(planner_data_)); - debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); - } - lk_pd.unlock(); // release planner_data_ planner_manager_->print(); @@ -535,16 +491,10 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - const uint16_t steering_factor_state = std::invoke([&intersection_flag]() { - if (intersection_flag) { - return SteeringFactor::TURNING; - } - return SteeringFactor::TRYING; - }); steering_factor_interface_ptr_->updateSteeringFactor( {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - PlanningBehavior::INTERSECTION, steering_factor_direction, steering_factor_state, ""); + PlanningBehavior::INTERSECTION, steering_factor_direction, SteeringFactor::TURNING, ""); } else { steering_factor_interface_ptr_->clearSteeringFactors(); } @@ -751,7 +701,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = utils::toPath(*path_candidate_ptr); + output = motion_utils::convertToPath( + *path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); @@ -771,7 +722,8 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( { // TODO(Horibe) do some error handling when path is not available. - auto path = output.path ? output.path : planner_data->prev_output_path; + auto path = !output.path.points.empty() ? std::make_shared(output.path) + : planner_data->prev_output_path; path->header = planner_data->route_handler->getRouteHeader(); path->header.stamp = this->now(); @@ -832,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; @@ -961,8 +914,8 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, planner_data_->drivable_area_expansion_parameters.max_bound_rate); updateParam( - parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); + parameters, DrivableAreaExpansionParameters::SMOOTHING_ARC_LENGTH_RANGE_PARAM, + planner_data_->drivable_area_expansion_parameters.arc_length_range); updateParam( parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, planner_data_->drivable_area_expansion_parameters.print_runtime); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c867bffc8e9bd..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) @@ -130,7 +131,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da /** * STEP1: get approved modules' output */ - const auto approved_modules_output = runApprovedModules(data); + auto approved_modules_output = runApprovedModules(data); /** * STEP2: check modules that need to be launched @@ -141,8 +142,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da * STEP3: if there is no module that need to be launched, return approved modules' output */ if (request_modules.empty()) { + const auto output = runKeepLastModules(data, approved_modules_output); processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; + return output; } /** @@ -150,24 +152,32 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da */ const auto [highest_priority_module, candidate_modules_output] = runRequestModules(request_modules, data, approved_modules_output); + + /** + * STEP5: run keep last approved modules after running candidate modules. + * NOTE: if no candidate module is launched, approved_modules_output used as input for keep + * last modules and return the result immediately. + */ + const auto output = runKeepLastModules( + data, highest_priority_module ? candidate_modules_output : approved_modules_output); if (!highest_priority_module) { processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; + return output; } /** - * STEP5: if the candidate module's modification is NOT approved yet, return the result. + * STEP6: if the candidate module's modification is NOT approved yet, return the result. * NOTE: the result is output of the candidate module, but the output path don't contains path * shape modification that needs approval. On the other hand, it could include velocity * profile modification. */ if (highest_priority_module->isWaitingApproval()) { processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return candidate_modules_output; + return output; } /** - * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_ + * STEP7: if the candidate module is approved, push the module into approved_module_ptrs_ */ addApprovedModule(highest_priority_module); clearCandidateModules(); @@ -178,7 +188,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", max_iteration_num_); processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return candidate_modules_output; + return output; } } @@ -198,7 +208,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da void PlannerManager::generateCombinedDrivableArea( BehaviorModuleOutput & output, const std::shared_ptr & data) const { - if (!output.path || output.path->points.empty()) { + if (output.path.points.empty()) { RCLCPP_ERROR_STREAM(logger_, "[generateCombinedDrivableArea] Output path is empty!"); return; } @@ -206,20 +216,20 @@ void PlannerManager::generateCombinedDrivableArea( const auto & di = output.drivable_area_info; constexpr double epsilon = 1e-3; - const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points); + const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path.points); const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (epsilon < std::abs(di.drivable_margin)) { // for single free space pull over utils::generateDrivableArea( - *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); + output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward); } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, + output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, is_driving_forward); } else { - const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); const auto & dp = data->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -228,19 +238,19 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( - *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, + output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, is_driving_forward); } // extract obstacles from drivable area - utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles); + utils::extractObstaclesFromDrivableArea(output.path, di.obstacles); } std::vector PlannerManager::getRequestModules( const BehaviorModuleOutput & previous_module_output) const { - if (!previous_module_output.path) { + if (previous_module_output.path.points.empty()) { RCLCPP_ERROR_STREAM( logger_, "Current module output is null. Skip candidate module check." << "\n - Approved module list: " << getNames(approved_module_ptrs_) @@ -272,22 +282,22 @@ std::vector PlannerManager::getRequestModules( // if there exists at least one approved module that is simultaneous but not always // executable. (only modules that are either always executable or simultaneous executable can // be added) - conditions.push_back( - {[&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + conditions.emplace_back( + [&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }); // Condition 3: do not add modules that are not always executable if there exists // at least one approved module that is neither always nor simultaneous executable. // (only modules that are always executable can be added) - conditions.push_back( - {[&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return false; }}); + conditions.emplace_back( + [&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }); bool skip_module = false; for (const auto & condition : conditions) { @@ -394,6 +404,19 @@ std::vector PlannerManager::getRequestModules( return request_modules; } +BehaviorModuleOutput PlannerManager::runKeepLastModules( + const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) const +{ + auto output = previous_output; + std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { + if (getManager(m)->isKeepLast()) { + output = run(m, data, output); + } + }); + + return output; +} + BehaviorModuleOutput PlannerManager::getReferencePath( const std::shared_ptr & data) const { @@ -484,22 +507,22 @@ std::pair PlannerManager::runRequestModule // Condition 3: Only modules that are always executable can be added // if there exists at least one executable module that is neither always nor simultaneous // executable. - conditions.push_back( - {[this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return false; }}); + conditions.emplace_back( + [this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }); // Condition 2: Only modules that are either always executable or simultaneous executable can be // added if there exists at least one executable module that is simultaneous but not always // executable. - conditions.push_back( - {[this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + conditions.emplace_back( + [this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }); for (const auto & condition : conditions) { const auto & find_block_module = condition.first; @@ -652,11 +675,13 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname(), output); + if (!getManager(m)->isKeepLast()) { + output = run(m, data, output); + results.emplace(m->name(), output); + } }); /** @@ -751,11 +776,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname().find("lane_change") != std::string::npos; }); - - if (success_lane_change) { + if (std::any_of(itr, approved_module_ptrs_.end(), [](const auto & m) { + return m->isRootLaneletToBeUpdated(); + })) { root_lanelet_ = updateRootLanelet(data); } @@ -839,7 +862,7 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) 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; }); + [](const auto & m) { return m->isRootLaneletToBeUpdated(); }); return lane_change_itr != approved_module_ptrs_.end(); }); if (is_lane_change_running) { @@ -886,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()); }; @@ -933,11 +952,18 @@ void PlannerManager::print() const string_stream << "\n" << std::fixed << std::setprecision(1); string_stream << "processing time : "; for (const auto & t : processing_time_) { - string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first - << ":" << std::setw(4) << std::right << t.second << "ms]\n" + string_stream << std::right << "[" << std::setw(static_cast(max_string_num) + 1) + << std::left << t.first << ":" << std::setw(4) << std::right << t.second + << "ms]\n" << std::setw(21); } + state_publisher_ptr_->publish("internal_state", string_stream.str()); + + if (!verbose_) { + return; + } + RCLCPP_INFO_STREAM(logger_, string_stream.str()); } @@ -962,7 +988,7 @@ std::shared_ptr PlannerManager::getDebugMsg() return debug_msg_ptr_; } -std::string PlannerManager::getNames(const std::vector & modules) const +std::string PlannerManager::getNames(const std::vector & modules) { std::stringstream ss; for (const auto & m : modules) { diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 0e253c0c73985..7ba934e873a8d 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include @@ -49,28 +50,18 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); - module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", - behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", - behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner_common/CMakeLists.txt index 5dbf81280f9b1..dbbd7c7cedc67 100644 --- a/planning/behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner_common/CMakeLists.txt @@ -4,7 +4,6 @@ project(behavior_path_planner_common) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) ament_auto_add_library(${PROJECT_NAME} SHARED @@ -23,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/drivable_area_expansion/footprints.cpp src/utils/parking_departure/geometric_parallel_parking.cpp src/utils/parking_departure/utils.cpp + src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp ) @@ -30,10 +30,6 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) -target_link_libraries(${PROJECT_NAME} - ${OpenCV_LIBRARIES} -) - if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities test/test_drivable_area_expansion.cpp diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md new file mode 100644 index 0000000000000..015ffcdfd8f3b --- /dev/null +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md @@ -0,0 +1,178 @@ +# Drivable Area design + +Drivable Area represents the area where ego vehicle can pass. + +## Purpose / Role + +In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function. + +## Assumption + +Our drivable area has several assumptions. + +- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes. + +- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections). + +- Both left and right bounds should cover the front of the path and the end of the path. + +## Limitations + +Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative. + +## Parameters for drivable area generation + +### Static expansion + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :--- | :----- | :------------------------------------------------------------------------- | :------------ | +| drivable_area_right_bound_offset | [m] | double | right offset length to expand drivable area | 5.0 | +| drivable_area_left_bound_offset | [m] | double | left offset length to expand drivable area | 5.0 | +| drivable_area_types_to_skip | [-] | string | linestring types (as defined in the lanelet map) that will not be expanded | road_border | + +### Dynamic expansion + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------- | :---- | :----------- | :------------------------------------------------------------------------------------------------------ | :--------------------------- | +| enabled | [-] | boolean | if true, dynamically expand the drivable area based on the path curvature | true | +| print_runtime | [-] | boolean | if true, runtime is logged by the node | true | +| max_expansion_distance | [m] | double | maximum distance by which the original drivable area can be expanded (no limit if set to 0) | 0.0 | +| smoothing.curvature_average_window | [-] | int | window size used for smoothing the curvatures using a moving window average | 3 | +| smoothing.max_bound_rate | [m/m] | double | maximum rate of change of the bound lateral distance over its arc length | 1.0 | +| smoothing.arc_length_range | [m] | double | arc length range where an expansion distance is initially applied | 2.0 | +| ego.extra_wheel_base | [m] | double | extra ego wheelbase | 0.0 | +| ego.extra_front_overhang | [m] | double | extra ego overhang | 0.5 | +| ego.extra_width | [m] | double | extra ego width | 1.0 | +| dynamic_objects.avoid | [-] | boolean | if true, the drivable area is not expanded in the predicted path of dynamic objects | true | +| dynamic_objects.extra_footprint_offset.front | [m] | double | extra length to add to the front of the ego footprint | 0.5 | +| dynamic_objects.extra_footprint_offset.rear | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | +| dynamic_objects.extra_footprint_offset.left | [m] | double | extra length to add to the left of the ego footprint | 0.5 | +| dynamic_objects.extra_footprint_offset.right | [m] | double | extra length to add to the rear of the ego footprint | 0.5 | +| path_preprocessing.max_arc_length | [m] | double | maximum arc length along the path where the ego footprint is projected (0.0 means no limit) | 100.0 | +| path_preprocessing.resample_interval | [m] | double | fixed interval between resampled path points (0.0 means path points are directly used) | 2.0 | +| path_preprocessing.reuse_max_deviation | [m] | double | if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused | 0.5 | +| avoid_linestring.types | [-] | string array | linestring types in the lanelet maps that will not be crossed when expanding the drivable area | ["road_border", "curbstone"] | +| avoid_linestring.distance | [m] | double | distance to keep between the drivable area and the linestrings to avoid | 0.0 | + +## Inner-workings / Algorithms + +This section gives details of the generation of the drivable area (`left_bound` and `right_bound`). + +### Drivable Lanes Generation + +Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Goal Planner`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. + +```cpp +struct DrivalbleLanes +{ + lanelet::ConstLanelet right_lanelet; // right most lane + lanelet::ConstLanelet left_lanelet; // left most lane + lanelet::ConstLanelets middle_lanelets; // middle lanes +}; +``` + +The image of the sorted drivable lanes is depicted in the following picture. + +![sorted_lanes](../images/drivable_area/sorted_lanes.drawio.svg) + +Note that, the order of drivable lanes become + +```cpp +drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5} +``` + +### Drivable Area Generation + +In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as + +```cpp +std::vector left_bound; +std::vector right_bound; +``` + +and each point of right bound and left bound has a position in the absolute coordinate system. + +![drivable_lines](../images/drivable_area/drivable_lines.drawio.svg) + +### Drivable Area Expansion + +#### Static Expansion + +Each module can statically expand the left and right bounds of the target lanes by the parameter defined values. +This enables large vehicles to pass narrow curve. The image of this process can be described as + +![expanded_lanes](../images/drivable_area/expanded_lanes.drawio.svg) + +Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane. + +#### Dynamic Expansion + +The drivable area can also be expanded dynamically based on a minimum width calculated from the path curvature and the ego vehicle's properties. +If static expansion is also enabled, the dynamic expansion will be done after the static expansion such that both expansions are applied. + +| Without dynamic expansion | With dynamic expansion | +| --------------------------------------------------------------------------- | ------------------------------------------------------------------------- | +| ![dynamic_expansion_off](../images/drivable_area/dynamic_expansion_off.png) | ![dynamic_expansion_on](../images/drivable_area/dynamic_expansion_on.png) | + +Next we detail the algorithm used to expand the drivable area bounds. + +##### 1 Calculate and smooth the path curvature + +To avoid sudden changes of the dynamically expanded drivable area, we first try to reuse as much of the previous path and its calculated curvatures as possible. +Previous path points and curvatures are reused up to the first previous path point that deviates from the new path by more than the `reuse_max_deviation` parameter. +At this stage, the path is also resampled according to the `resampled_interval` and cropped according to the `max_arc_length`. +With the resulting preprocessed path points and previous curvatures, curvatures of the new path points are calculated using the 3 points method and smoothed using a moving window average with window size `curvature_average_window`. + +##### 2 For each path point, calculate the closest bound segment and the minimum drivable area width + +Each path point is projected on the original left and right drivable area bounds to calculate its corresponding bound index, original distance from the bounds, and the projected point. +Additionally, for each path point, the minimum drivable area width is calculated using the following equation: +$$ W = \frac{a² + 2 al + 2kw + l² + w²}{2k + w}$$ +Where $W$ is the minimum drivable area width, $a$, is the front overhang of ego, $l$ is the wheelbase of ego, $w$ is the width of ego, and $k$ is the path curvature. +This equation was derived from the work of [Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021](https://www.sae.org/publications/technical-papers/content/2021-01-0099/). + +![min width](../images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg) + +##### 3 Calculate maximum expansion distances of each bound point based on dynamic objects and linestring of the vector map (optional) + +For each drivable area bound point, we calculate its maximum expansion distance as its distance to the closest "obstacle" (either a map linestring with type `avoid_linestrings.type`, or a dynamic object footprint if `dynamic_objects.avoid` is set to `true`). +If `max_expansion_distance` is not `0.0`, it is use here if smaller than the distance to the closest obstacle. + +![max distances](../images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg) + +##### 4 Calculate by how much each bound point should be pushed away from the path + +For each bound point, a shift distance is calculated. +such that the resulting width between corresponding left and right bound points is as close as possible to the minimum width calculated in step 2 but the individual shift distance stays bellow the previously calculated maximum expansion distance. + +![expansion distances](../images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg) + +##### 5 Shift bound points by the values calculated in step 4 and remove all loops in the resulting bound + +Finally, each bound point is shifted away from the path by the distance calculated in step 4. +Once all points have been shifted, loops are removed from the bound and we obtain our final expanded drivable area. + +| | | +| ------------------------------------------------------------------------------ | ------------------------------------------------------------------------ | +| ![expansion](../images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg) | ![result](../images/drivable_area/DynamicDrivableArea-Result.drawio.svg) | + +### Visualizing maximum drivable area (Debug) + +Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. + +For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user. + +To debug the issue, the maximum drivable area boundary can be visualized. + +![drivable_area_boundary_marker1](../images/drivable_area/drivable_area_boundary_marker_example1.png) + +![drivable_area_boundary_marker2](../images/drivable_area/drivable_area_boundary_marker_example2.png) + +The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area` + +#### Expansion with hatched road markings area + +If the hatched road markings area is defined in the lanelet map, the area can be used as a drivable area. +Since the area is expressed as a polygon format of Lanelet2, several steps are required for correct expansion. + +![hatched_road_markings_drivable_area_expansion](../images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md similarity index 97% rename from planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md rename to planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md index 7784f96543ea6..2a22db5597241 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md @@ -1,6 +1,6 @@ # Path Generation design -This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp). +This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp). ## Overview @@ -8,7 +8,7 @@ The base idea of the path generation in lane change and avoidance is to smoothly The figure below explains how the application of a constant lateral jerk $l^{'''}(s)$ can be used to induce lateral shifting. In order to comply with the limits on lateral acceleration and velocity, zero-jerk time is employed in the figure ( $T_a$ and $T_v$ ). In each interval where constant jerk is applied, the shift position $l(s)$ can be characterized by a third-degree polynomial. Therefore the shift length from the reference path can then be calculated by combining spline curves. -![path-shifter](../image/path_shifter.png) +![path-shifter](../images/path_shifter/path_shifter.png) Note that, due to the rarity of the $T_v$ in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md similarity index 93% rename from planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md rename to planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md index 30e2093cb465e..4733fe7832da6 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md @@ -22,7 +22,7 @@ Currently the yaw angle of each point of predicted paths of a target object does The flow of the safety check algorithm is described in the following explanations. -![safety_check_flow](../image/path_safety_checker/safety_check_flow.drawio.svg) +![safety_check_flow](../images/path_safety_checker/safety_check_flow.drawio.svg) Here we explain each step of the algorithm flow. @@ -38,7 +38,7 @@ With the interpolated pose obtained in the step.1, we check if the object and eg After the overlap check, it starts to perform the safety check for the broader range. In this step, it judges if ego or target object is in front of the other vehicle. We use arc length of the front point of each object along the given path to judge which one is in front of the other. In the following example, target object (red rectangle) is running in front of the ego vehicle (black rectangle). -![front_object](../image/path_safety_checker/front_object.drawio.svg) +![front_object](../images/path_safety_checker/front_object.drawio.svg) #### 4. Calculate RSS distance @@ -54,7 +54,7 @@ where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively a In this step, we compute extended ego and target object polygons. The extended polygons can be described as: -![extended_polygons](../image/path_safety_checker/extended_polygons.drawio.svg) +![extended_polygons](../images/path_safety_checker/extended_polygons.drawio.svg) As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md similarity index 86% rename from planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md rename to planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md index 8b64db997aec6..c183e0627a674 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md @@ -35,7 +35,7 @@ Note that the default values for `turn_signal_intersection_search_distance` and In this algorithm, it assumes that each blinker has two sections, which are `desired section` and `required section`. The image of these two sections are depicted in the following diagram. -![section_definition](../image/turn_signal_decider/sections_definition.drawio.svg) +![section_definition](../images/turn_signal_decider/sections_definition.drawio.svg) These two sections have the following meanings. @@ -51,7 +51,7 @@ These two sections have the following meanings. When turning on the blinker, it decides whether or not to turn on the specified blinker based on the distance from the front of the ego vehicle to the start point of each section. Conversely, when turning off the blinker, it calculates the distance from the base link of the ego vehicle to the end point of each section and decide whether or not to turn it off based on that. -![activation_distance](../image/turn_signal_decider/activation_distance.drawio.svg) +![activation_distance](../images/turn_signal_decider/activation_distance.drawio.svg) For left turn, right turn, avoidance, lane change, goal planner and pull out, we define these two sections, which are elaborated in the following part. @@ -71,7 +71,7 @@ Turn signal decider checks each lanelet on the map if it has `turn_direction` in - required end point The earliest point that satisfies the following condition. $\theta - \theta_{\textrm{end}} < \delta$, where $\theta_{\textrm{end}}$ is yaw angle of the terminal point of the lanelet, $\theta$ is the angle of a required end point and $\delta$ is the threshold defined by the user. -![section_turn_signal](../image/turn_signal_decider/left_right_turn.drawio.svg) +![section_turn_signal](../images/turn_signal_decider/left_right_turn.drawio.svg) #### 2. Avoidance @@ -91,7 +91,7 @@ First section - required end point Shift complete point same as the desired end point. -![section_first_avoidance](../image/turn_signal_decider/avoidance_first_section.drawio.svg) +![section_first_avoidance](../images/turn_signal_decider/avoidance_first_section.drawio.svg) Second section @@ -107,7 +107,7 @@ Second section - required end point Avoidance terminal point. -![section_second_avoidance](../image/turn_signal_decider/avoidance_second_section.drawio.svg) +![section_second_avoidance](../images/turn_signal_decider/avoidance_second_section.drawio.svg) #### 3. Lane Change @@ -123,7 +123,7 @@ Second section - required end point Cross point with lane change path and boundary line of the adjacent lane. -![section_lane_change](../image/turn_signal_decider/lane_change.drawio.svg) +![section_lane_change](../images/turn_signal_decider/lane_change.drawio.svg) #### 4. Pull out @@ -139,7 +139,7 @@ Second section - required end point Terminal point of the path of pull out. -![section_pull_out](../image/turn_signal_decider/pull_out.drawio.svg) +![section_pull_out](../images/turn_signal_decider/pull_out.drawio.svg) #### 5. Goal Planner @@ -155,24 +155,24 @@ Second section - required end point Terminal point of the path of pull over. -![section_pull_over](../image/turn_signal_decider/pull_over.drawio.svg) +![section_pull_over](../images/turn_signal_decider/pull_over.drawio.svg) When it comes to handle several blinkers, it gives priority to the first blinker that comes first. However, this rule sometimes activate unnatural blinkers, so turn signal decider uses the following five rules to decide the necessary turn signal. - pattern1 - ![pattern1](../image/turn_signal_decider/pattern1.drawio.svg) + ![pattern1](../images/turn_signal_decider/pattern1.drawio.svg) - pattern2 - ![pattern2](../image/turn_signal_decider/pattern2.drawio.svg) + ![pattern2](../images/turn_signal_decider/pattern2.drawio.svg) - pattern3 - ![pattern3](../image/turn_signal_decider/pattern3.drawio.svg) + ![pattern3](../images/turn_signal_decider/pattern3.drawio.svg) - pattern4 - ![pattern4](../image/turn_signal_decider/pattern4.drawio.svg) + ![pattern4](../images/turn_signal_decider/pattern4.drawio.svg) - pattern5 - ![pattern5](../image/turn_signal_decider/pattern5.drawio.svg) + ![pattern5](../images/turn_signal_decider/pattern5.drawio.svg) Based on these five rules, turn signal decider can solve `blinker conflicts`. The following pictures show some examples of this kind of conflicts. @@ -180,22 +180,22 @@ Based on these five rules, turn signal decider can solve `blinker conflicts`. Th In this scenario, ego vehicle has to pass several turns that are close each other. Since this pattern can be solved by the pattern1 rule, the overall result is depicted in the following picture. -![continuous_turns](../image/turn_signal_decider/continuous_turns.drawio.svg) +![continuous_turns](../images/turn_signal_decider/continuous_turns.drawio.svg) #### - Avoidance with left turn (1) In this scene, ego vehicle has to deal with the obstacle that is on its original path as well as make a left turn. The overall result can be varied by the position of the obstacle, but the image of the result is described in the following picture. -![avoidance_and_turn](../image/turn_signal_decider/avoidance_and_turn.drawio.svg) +![avoidance_and_turn](../images/turn_signal_decider/avoidance_and_turn.drawio.svg) #### - Avoidance with left turn (2) Same as the previous scenario, ego vehicle has to avoid the obstacle as well as make a turn left. However, in this scene, the obstacle is parked after the intersection. Similar to the previous one, the overall result can be varied by the position of the obstacle, but the image of the result is described in the following picture. -![avoidance_and_turn](../image/turn_signal_decider/avoidance_and_turn2.drawio.svg) +![avoidance_and_turn](../images/turn_signal_decider/avoidance_and_turn2.drawio.svg) #### - Lane change and left turn In this scenario, ego vehicle has to do lane change before making a left turn. In the following example, ego vehicle does not activate left turn signal until it reaches the end point of the lane change path. -![avoidance_and_turn](../image/turn_signal_decider/lane_change_and_turn.drawio.svg) +![avoidance_and_turn](../images/turn_signal_decider/lane_change_and_turn.drawio.svg) diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg new file mode 100644 index 0000000000000..163e5fbba719a --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg @@ -0,0 +1,106 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + + + + + + + + + + +
+
+
+
+ Maximum expansion distance +
+
+
+
+
+ + Maximum expansion distance + + + + + +
+
+
+
+ Obstacle +
+
+
+
+
+ + Obstacle + + + + + + + + + +
+
+
Expansion distances
+
+
+ + Expansion distances + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg new file mode 100644 index 0000000000000..a2b11b151db8c --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg @@ -0,0 +1,95 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + +
+
+
Expansion distances
+
+
+ + Expansion distances + + + + + + + + + + + + + + + +
+
+
Bound points
+
+
+ + Bound points + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg new file mode 100644 index 0000000000000..09a9260475dff --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg @@ -0,0 +1,56 @@ + + + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + +
+
+
+ Drivable area bounds +
+
+
+ + Drivable area bounds + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg new file mode 100644 index 0000000000000..7e1a823008fc8 --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg @@ -0,0 +1,84 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + + + + + + + + + + +
+
+
+
+ Maximum expansion distance +
+
+
+
+
+ + Maximum expansion distance + + + + + +
+
+
+
+ Obstacle +
+
+
+
+
+ + Obstacle + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg new file mode 100644 index 0000000000000..e6bc7a0137458 --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg @@ -0,0 +1,64 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + + + + + + + + + + +
+
+
+
+ Minimum drivable area width +
+
+
+
+
+ + Minimum drivable area width + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg new file mode 100644 index 0000000000000..9194cfc584b16 --- /dev/null +++ b/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg @@ -0,0 +1,59 @@ + + + + + + + + + + + +
+
+
Path points
+
+
+ + Path points + + + + + + +
+
+
Bound points
+
+
+ + Bound points + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png b/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png rename to planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png b/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png rename to planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png new file mode 100644 index 0000000000000..49b4b382c344b Binary files /dev/null and b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png differ diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png new file mode 100644 index 0000000000000..9a9dbbc7e8180 Binary files /dev/null and b/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png differ diff --git a/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg diff --git a/planning/behavior_path_planner/image/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg diff --git a/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg b/planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg rename to planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg diff --git a/planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg b/planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg rename to planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg diff --git a/planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg b/planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg rename to planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg diff --git a/planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg b/planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg rename to planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg diff --git a/planning/behavior_path_planner/image/path_shifter.png b/planning/behavior_path_planner_common/images/path_shifter/path_shifter.png similarity index 100% rename from planning/behavior_path_planner/image/path_shifter.png rename to planning/behavior_path_planner_common/images/path_shifter/path_shifter.png diff --git a/planning/behavior_path_planner/image/turn_signal_decider/activation_distance.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/activation_distance.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn2.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_and_turn2.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_first_section.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_first_section.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/avoidance_second_section.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/avoidance_second_section.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/continuous_turns.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/continuous_turns.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/lane_change.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/lane_change.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/lane_change_and_turn.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/lane_change_and_turn.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/left_right_turn.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/left_right_turn.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern1.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern1.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern2.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern2.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern3.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern3.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern4.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern4.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pattern5.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pattern5.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pull_out.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pull_out.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/pull_over.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/pull_over.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg diff --git a/planning/behavior_path_planner/image/turn_signal_decider/sections_definition.drawio.svg b/planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/turn_signal_decider/sections_definition.drawio.svg rename to planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 3dcfdcc2bdcef..31dc117fbce35 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -21,7 +21,8 @@ #include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include +#include #include #include @@ -38,12 +39,11 @@ #include #include -#include - #include #include #include #include +#include #include namespace behavior_path_planner @@ -100,7 +100,7 @@ struct DrivableAreaInfo { geometry_msgs::msg::Pose pose; tier4_autoware_utils::Polygon2d poly; - bool is_left; + bool is_left{true}; }; std::vector drivable_lanes{}; std::vector obstacles{}; // obstacles to extract from the drivable area @@ -119,10 +119,10 @@ struct BehaviorModuleOutput BehaviorModuleOutput() = default; // path planed by module - PlanResult path{}; + PathWithLaneId path{}; // reference path planed by module - PlanResult reference_path{}; + PathWithLaneId reference_path{}; TurnSignalInfo turn_signal_info{}; @@ -136,7 +136,7 @@ struct BehaviorModuleOutput struct CandidateOutput { CandidateOutput() = default; - explicit CandidateOutput(const PathWithLaneId & path) : path_candidate{path} {} + explicit CandidateOutput(PathWithLaneId path) : path_candidate{std::move(path)} {} PathWithLaneId path_candidate{}; double lateral_shift{0.0}; double start_distance_to_path_change{std::numeric_limits::lowest()}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index a5e2c3245f3c3..1a8c8241abe1a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -111,21 +111,6 @@ class SceneModuleInterface virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; - /** - * @brief Set the current_state_ based on updateState output. - */ - virtual void updateCurrentState() - { - const auto print = [this](const auto & from, const auto & to) { - RCLCPP_DEBUG( - getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); - }; - - const auto & from = current_state_; - current_state_ = updateState(); - print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); - } - /** * @brief Return true if the module has request for execution (not necessarily feasible) */ @@ -159,6 +144,21 @@ class SceneModuleInterface return isWaitingApproval() ? planWaitingApproval() : plan(); } + /** + * @brief Set the current_state_ based on updateState output. + */ + void updateCurrentState() + { + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG( + getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); + }; + + const auto & from = current_state_; + current_state_ = updateState(); + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); + } + /** * @brief Called on the first time when the module goes into RUNNING. */ @@ -259,6 +259,8 @@ class SceneModuleInterface return is_waiting_approval_ || current_state_ == ModuleStatus::WAITING_APPROVAL; } + virtual bool isRootLaneletToBeUpdated() const { return false; } + bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } PlanResult getPathCandidate() const { return path_candidate_; } @@ -360,9 +362,83 @@ class SceneModuleInterface return existApprovedRequest(); } + /** + * @brief Return SUCCESS if plan is not needed or plan is successfully finished, + * FAILURE if plan has failed, RUNNING if plan is on going. + * These condition is to be implemented in each modules. + */ + ModuleStatus updateState() + { + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + if (current_state_ == ModuleStatus::IDLE) { + if (canTransitIdleToRunningState()) { + log_debug_throttled("transiting from IDLE to RUNNING"); + return ModuleStatus::RUNNING; + } + + log_debug_throttled("transiting from IDLE to IDLE"); + return ModuleStatus::IDLE; + } + + if (current_state_ == ModuleStatus::RUNNING) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from RUNNING to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from RUNNING to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalState()) { + log_debug_throttled("transiting from RUNNING to WAITING_APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + log_debug_throttled("transiting from RUNNING to RUNNING"); + return ModuleStatus::RUNNING; + } + + if (current_state_ == ModuleStatus::WAITING_APPROVAL) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalToRunningState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to RUNNING"); + return ModuleStatus::RUNNING; + } + + log_debug_throttled("transiting from WAITING_APPROVAL to WAITING APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + if (current_state_ == ModuleStatus::SUCCESS) { + log_debug_throttled("already SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (current_state_ == ModuleStatus::FAILURE) { + log_debug_throttled("already FAILURE"); + return ModuleStatus::FAILURE; + } + + log_debug_throttled("already IDLE"); + return ModuleStatus::IDLE; + } + std::string name_; - rclcpp::Logger logger_; + ModuleStatus current_state_{ModuleStatus::IDLE}; BehaviorModuleOutput previous_module_output_; @@ -405,7 +481,7 @@ class SceneModuleInterface virtual BehaviorModuleOutput planWaitingApproval() { path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return getPreviousModuleOutput(); } @@ -442,64 +518,6 @@ class SceneModuleInterface } } - /** - * @brief Return SUCCESS if plan is not needed or plan is successfully finished, - * FAILURE if plan has failed, RUNNING if plan is on going. - * These condition is to be implemented in each modules. - */ - virtual ModuleStatus updateState() - { - if (current_state_ == ModuleStatus::IDLE) { - if (canTransitIdleToRunningState()) { - return ModuleStatus::RUNNING; - } - - return ModuleStatus::IDLE; - } - - if (current_state_ == ModuleStatus::RUNNING) { - if (canTransitSuccessState()) { - return ModuleStatus::SUCCESS; - } - - if (canTransitFailureState()) { - return ModuleStatus::FAILURE; - } - - if (canTransitWaitingApprovalState()) { - return ModuleStatus::WAITING_APPROVAL; - } - - return ModuleStatus::RUNNING; - } - - if (current_state_ == ModuleStatus::WAITING_APPROVAL) { - if (canTransitSuccessState()) { - return ModuleStatus::SUCCESS; - } - - if (canTransitFailureState()) { - return ModuleStatus::FAILURE; - } - - if (canTransitWaitingApprovalToRunningState()) { - return ModuleStatus::RUNNING; - } - - return ModuleStatus::WAITING_APPROVAL; - } - - if (current_state_ == ModuleStatus::SUCCESS) { - return ModuleStatus::SUCCESS; - } - - if (current_state_ == ModuleStatus::FAILURE) { - return ModuleStatus::FAILURE; - } - - return ModuleStatus::IDLE; - } - /** * @brief Return true if the activation command is received from the RTC interface. * If no RTC interface is registered, return true. @@ -581,6 +599,8 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; @@ -592,8 +612,6 @@ class SceneModuleInterface PlanResult path_candidate_; PlanResult path_reference_; - ModuleStatus current_state_{ModuleStatus::IDLE}; - std::unordered_map> rtc_interface_ptr_map_; std::unordered_map> 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 95976bf117096..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 @@ -19,10 +19,13 @@ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" -#include +#include +#include +#include #include +#include #include #include #include @@ -44,7 +47,11 @@ using SceneModuleObserver = std::weak_ptr; class SceneModuleManagerInterface { public: - explicit SceneModuleManagerInterface(const std::string & name) : name_{name} {} + SceneModuleManagerInterface(const SceneModuleManagerInterface &) = delete; + SceneModuleManagerInterface(SceneModuleManagerInterface &&) = delete; + SceneModuleManagerInterface & operator=(const SceneModuleManagerInterface &) = delete; + SceneModuleManagerInterface & operator=(SceneModuleManagerInterface &&) = delete; + explicit SceneModuleManagerInterface(std::string name) : name_{std::move(name)} {} virtual ~SceneModuleManagerInterface() = default; @@ -261,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( @@ -275,7 +289,7 @@ class SceneModuleManagerInterface for (const auto & rtc_type : rtc_types) { const auto snake_case_name = utils::convertToSnakeCase(name_); const auto rtc_interface_name = - rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; + rtc_type.empty() ? snake_case_name : snake_case_name + "_" + rtc_type; rtc_interface_ptr_map_.emplace( rtc_type, std::make_shared(node, rtc_interface_name, config_.enable_rtc)); objects_of_interest_marker_interface_ptr_map_.emplace( @@ -298,7 +312,7 @@ class SceneModuleManagerInterface virtual std::unique_ptr createNewSceneModuleInstance() = 0; - rclcpp::Node * node_; + rclcpp::Node * node_ = nullptr; rclcpp::Publisher::SharedPtr pub_info_marker_; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp index fd3ce0097c2eb..fb0cb97a3db95 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp @@ -37,7 +37,7 @@ class SteeringFactorInterface void updateSteeringFactor( const std::array & poses, const std::array distances, const std::string & behavior, const uint16_t direction, const uint16_t status, - const std::string detail); + const std::string & detail); void clearSteeringFactors(); private: diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp index 95dcff8602398..1c066ff744f46 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp @@ -16,7 +16,7 @@ #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "std_msgs/msg/color_rgba.hpp" +#include "std_msgs/msg/detail/color_rgba__struct.hpp" #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index cfaa8de9b2fb9..6426f16a44259 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -18,6 +18,8 @@ #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 + #include #include #include @@ -56,6 +58,18 @@ inline int64_t bitShift(int64_t original_id) return original_id << (sizeof(int32_t) * 8 / 2); } +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info); + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b); + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); @@ -81,8 +95,6 @@ MarkerArray createLaneletsAreaMarkerArray( const std::vector & lanelets, std::string && ns, const float & r, const float & g, const float & b); -MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings); - MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b, const float & w = 0.3); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 7832dd4e53cfc..811c89f61c066 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -15,12 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ -#include #include -#include -#include - struct ModuleConfigParameters { bool enable_module{false}; @@ -32,47 +28,6 @@ struct ModuleConfigParameters uint8_t max_module_size{0}; }; -struct LateralAccelerationMap -{ - std::vector base_vel; - std::vector base_min_acc; - std::vector base_max_acc; - - void add(const double velocity, const double min_acc, const double max_acc) - { - if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { - return; - } - - size_t idx = 0; - for (size_t i = 0; i < base_vel.size(); ++i) { - if (velocity < base_vel.at(i)) { - break; - } - idx = i + 1; - } - - base_vel.insert(base_vel.begin() + idx, velocity); - base_min_acc.insert(base_min_acc.begin() + idx, min_acc); - base_max_acc.insert(base_max_acc.begin() + idx, max_acc); - } - - std::pair find(const double velocity) const - { - if (!base_vel.empty() && velocity < base_vel.front()) { - return std::make_pair(base_min_acc.front(), base_max_acc.front()); - } - if (!base_vel.empty() && velocity > base_vel.back()) { - return std::make_pair(base_min_acc.back(), base_max_acc.back()); - } - - const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); - - return std::make_pair(min_acc, max_acc); - } -}; - struct BehaviorPathPlannerParameters { bool verbose; @@ -81,8 +36,6 @@ struct BehaviorPathPlannerParameters double backward_path_length; double forward_path_length; - double backward_length_buffer_for_end_of_lane; - double backward_length_buffer_for_blocking_object; double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; @@ -90,13 +43,6 @@ struct BehaviorPathPlannerParameters double min_acc; double max_acc; - // lane change parameters - double lane_changing_lateral_jerk{0.5}; - double minimum_lane_changing_velocity{5.6}; - double lane_change_prepare_duration{4.0}; - double lane_change_finish_judge_buffer{3.0}; - LateralAccelerationMap lane_change_lat_acc_map; - double minimum_pull_over_length; double minimum_pull_out_length; double drivable_area_resolution; @@ -130,9 +76,6 @@ struct BehaviorPathPlannerParameters double right_over_hang; double base_link2front; double base_link2rear; - - // maximum drivable area visualization - bool visualize_maximum_drivable_area; }; #endif // BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index 372041c7fb586..cd639b5e81092 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -39,7 +39,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; -const std::map signal_map = { +const std::map g_signal_map = { {"left", TurnIndicatorsCommand::ENABLE_LEFT}, {"right", TurnIndicatorsCommand::ENABLE_RIGHT}, {"straight", TurnIndicatorsCommand::DISABLE}, 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/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp index d0eec8f2b8901..7e4cf94c890d8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -35,14 +35,14 @@ void expand_drivable_area( PathWithLaneId & path, const std::shared_ptr planner_data); -/// @brief prepare path poses and try to reuse their previously calculated curvatures +/// @brief try to reuse the previous path poses and their previously calculated curvatures /// @details poses are reused if they do not deviate too much from the current path /// @param [in] path input path /// @param [inout] prev_poses previous poses to reuse /// @param [inout] prev_curvatures previous curvatures to reuse /// @param [in] ego_point current ego point /// @param [in] params parameters for reuse criteria and resampling interval -void update_path_poses_and_previous_curvatures( +void reuse_previous_poses( const PathWithLaneId & path, std::vector & prev_poses, std::vector & prev_curvatures, const Point & ego_point, const DrivableAreaExpansionParameters & params); @@ -57,6 +57,36 @@ void update_path_poses_and_previous_curvatures( double calculate_minimum_lane_width( const double curvature_radius, const DrivableAreaExpansionParameters & params); +/// @brief calculate mappings between path poses and the given drivable area bound +/// @param [inout] expansion expansion data to update with the mapping +/// @param [in] path_poses path poses +/// @param [in] bound drivable area bound +/// @param [in] Side left or right side +void calculate_bound_index_mappings( + Expansion & expansion, const std::vector & path_poses, const std::vector & bound, + const Side side); + +/// @brief apply expansion distances to all bound points within the given arc length range +/// @param [inout] expansion expansion data to update +/// @param [in] bound drivable area bound +/// @param [in] arc_length_range [m] arc length range where the expansion distances are also applied +/// @param [in] Side left or right side +void apply_arc_length_range_smoothing( + Expansion & expansion, const std::vector & bound, const double arc_length_range, + const Side side); + +/// @brief calculate minimum lane widths and mappings between path and and drivable area bounds +/// @param [in] path_poses path poses +/// @param [in] left_bound left drivable area bound +/// @param [in] right_bound right drivable area bound +/// @param [in] curvatures curvatures at each path point +/// @param [in] params parameters with the vehicle dimensions used to calculate the min lane width +/// @return expansion data (path->bound mappings, min lane widths, ...) +Expansion calculate_expansion( + const std::vector & path_poses, const std::vector & left_bound, + const std::vector & right_bound, const std::vector & curvatures, + const DrivableAreaExpansionParameters & params); + /// @brief smooth the bound by applying a limit on its rate of change /// @details rate of change is the lateral distance from the path over the arc length along the path /// @param [inout] bound_distances bound distances (lateral distance from the path) @@ -66,16 +96,16 @@ void apply_bound_change_rate_limit( std::vector & distances, const std::vector & bound, const double max_rate); /// @brief calculate the maximum distance by which a bound can be expanded -/// @param [in] path_poses input path /// @param [in] bound bound points /// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree /// @param [in] uncrossable_polygons polygons that limit the bound expansion /// @param [in] params parameters with the buffer distance to keep with lines, /// and the static maximum expansion distance +/// @param [in] Side left or right side std::vector calculate_maximum_distance( - const std::vector & path_poses, const std::vector bound, - const SegmentRtree & uncrossable_lines, const std::vector & uncrossable_polygons, - const DrivableAreaExpansionParameters & params); + const std::vector & bound, const SegmentRtree & uncrossable_lines, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params, const Side side); /// @brief expand a bound by the given lateral distances away from the path /// @param [inout] bound bound points to expand @@ -85,6 +115,14 @@ void expand_bound( std::vector & bound, const std::vector & path_poses, const std::vector & distances); +/// @brief calculate the expansion distances of the left and right drivable area bounds +/// @param [inout] expansion expansion data to be updated with the left/right expansion distances +/// @param [in] max_left_expansions maximum left expansion distances +/// @param [in] max_right_expansions maximum right expansion distances +void calculate_expansion_distances( + Expansion & expansion, const std::vector & max_left_expansions, + const std::vector & max_right_expansions); + /// @brief calculate smoothed curvatures /// @details smoothing is done using a moving average /// @param [in] poses input poses used to calculate the curvatures diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index 297092d0cdd68..2a4343e2c3f6a 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -22,13 +22,6 @@ #include #include -#include - -#include -#include -#include -#include -#include namespace drivable_area_expansion { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 6d1497397ad27..055e20a4cda02 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -55,8 +55,8 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.smoothing.curvature_average_window"; static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM = "dynamic_expansion.smoothing.max_bound_rate"; - static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM = - "dynamic_expansion.smoothing.extra_arc_length"; + static constexpr auto SMOOTHING_ARC_LENGTH_RANGE_PARAM = + "dynamic_expansion.smoothing.arc_length_range"; static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; // static expansion @@ -78,7 +78,7 @@ struct DrivableAreaExpansionParameters double max_expansion_distance{}; double max_path_arc_length{}; double resample_interval{}; - double extra_arc_length{}; + double arc_length_range{}; double max_reuse_deviation{}; bool avoid_dynamic_objects{}; bool print_runtime{}; @@ -103,7 +103,7 @@ struct DrivableAreaExpansionParameters extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); - extra_arc_length = node.declare_parameter(SMOOTHING_EXTRA_ARC_LENGTH_PARAM); + arc_length_range = node.declare_parameter(SMOOTHING_ARC_LENGTH_RANGE_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 4c1469d46e03a..2aeac123623ce 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -19,14 +19,13 @@ #include -#include +#include #include namespace drivable_area_expansion { /// @brief project a point to a segment -/// @details the distance is signed based on the side of the point: left=positive, right=negative /// @param p point to project on the segment /// @param p1 first segment point /// @param p2 second segment point @@ -37,22 +36,18 @@ inline PointDistance point_to_segment_projection( const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; - const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); - const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; - const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); - if (c1 <= 0) return {p1, boost::geometry::distance(p, p1) * dist_sign}; + if (c1 <= 0) return {p1, boost::geometry::distance(p, p1)}; const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); - if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; + if (c2 <= c1) return {p2, boost::geometry::distance(p, p2)}; const auto projection = p1 + (p2_vec * c1 / c2); const auto projection_point = Point2d{projection.x(), projection.y()}; - return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; + return {projection_point, boost::geometry::distance(p, projection_point)}; } /// @brief project a point to a line -/// @details the distance is signed based on the side of the point: left=positive, right=negative /// @param p point to project on the line /// @param p1 first line point /// @param p2 second line point @@ -63,14 +58,11 @@ inline PointDistance point_to_line_projection( const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; - const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); - const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; - const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); const auto projection = p1 + (p2_vec * c1 / c2); const auto projection_point = Point2d{projection.x(), projection.y()}; - return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; + return {projection_point, boost::geometry::distance(p, projection_point)}; } /// @brief project a point to a linestring @@ -84,7 +76,7 @@ inline Projection point_to_linestring_projection(const Point2d & p, const LineSt auto arc_length = 0.0; for (auto ls_it = ls.begin(); std::next(ls_it) != ls.end(); ++ls_it) { const auto pd = point_to_segment_projection(p, *ls_it, *(ls_it + 1)); - if (std::abs(pd.distance) < std::abs(closest.distance)) { + if (pd.distance < closest.distance) { closest.projected_point = pd.point; closest.distance = pd.distance; closest.arc_length = arc_length + boost::geometry::distance(*ls_it, pd.point); @@ -141,13 +133,11 @@ inline Segment2d linestring_to_point_projection( const auto lerp_ratio = (arc_length - prev_arc_length) / (curr_arc_length - prev_arc_length); const auto base_point = lerp_point(*std::prev(ls_iterator), *ls_iterator, lerp_ratio); - if (distance == 0.0) - return {base_point, base_point}; - else if (lerp_ratio >= 1.0) // base point is beyond the 2nd segment point -> calculate normal in - // the other direction + if (distance == 0.0) return {base_point, base_point}; + if (lerp_ratio >= 1.0) // base point is beyond the 2nd segment point -> calculate normal in + // the other direction return {base_point, normal_at_distance(base_point, *std::prev(ls_iterator), -distance)}; - else - return {base_point, normal_at_distance(base_point, *ls_iterator, distance)}; + return {base_point, normal_at_distance(base_point, *ls_iterator, distance)}; } /// @brief create a sub linestring between the given arc lengths diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 284b4aad20a6a..9053da45708a0 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -47,9 +47,6 @@ void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, const bool is_driving_forward = true); -lanelet::ConstLineStrings3d getMaximumDrivableArea( - const std::shared_ptr & planner_data); - /** * @brief Expand the borders of the given lanelets * @param [in] drivable_lanes lanelets to expand diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index eae53b027655c..e438105c6645e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -24,6 +24,9 @@ #include +#include +#include + namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -41,20 +44,31 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::Segment2d; -typedef boost::geometry::index::rtree> SegmentRtree; +using SegmentRtree = boost::geometry::index::rtree>; struct PointDistance { - Point2d point; - double distance; + Point2d point{}; + double distance{}; }; struct Projection { - Point2d projected_point; - double distance; - double arc_length; + Point2d projected_point{}; + double distance{}; + double arc_length{}; }; enum Side { LEFT, RIGHT }; - +struct Expansion +{ + // mappings from bound index + std::vector left_distances; + std::vector right_distances; + // mappings from path index + std::vector left_bound_indexes; + std::vector left_projections; + std::vector right_bound_indexes; + std::vector right_projections; + std::vector min_lane_widths; +}; } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp similarity index 79% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 91d99fcd4a0d6..5b1e9b4b4c419 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ #include @@ -21,14 +21,6 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include namespace behavior_path_planner @@ -74,8 +66,8 @@ struct OccupancyGridMapParam VehicleShape vehicle_shape; // costmap configs - int theta_size; // discretized angle table size [-] - int obstacle_threshold; // obstacle threshold on grid [-] + int theta_size{0}; // discretized angle table size [-] + int obstacle_threshold{0}; // obstacle threshold on grid [-] }; struct PlannerWaypoint @@ -93,7 +85,12 @@ struct PlannerWaypoints class OccupancyGridBasedCollisionDetector { public: - OccupancyGridBasedCollisionDetector() {} + OccupancyGridBasedCollisionDetector() = default; + OccupancyGridBasedCollisionDetector(const OccupancyGridBasedCollisionDetector &) = default; + OccupancyGridBasedCollisionDetector(OccupancyGridBasedCollisionDetector &&) = delete; + OccupancyGridBasedCollisionDetector & operator=(const OccupancyGridBasedCollisionDetector &) = + default; + OccupancyGridBasedCollisionDetector & operator=(OccupancyGridBasedCollisionDetector &&) = delete; void setParam(const OccupancyGridMapParam & param) { param_ = param; }; OccupancyGridMapParam getParam() const { return param_; }; void setMap(const nav_msgs::msg::OccupancyGrid & costmap); @@ -106,7 +103,7 @@ class OccupancyGridBasedCollisionDetector const bool check_out_of_range) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const; - virtual ~OccupancyGridBasedCollisionDetector() {} + virtual ~OccupancyGridBasedCollisionDetector() = default; protected: void computeCollisionIndexes(int theta_index, std::vector & indexes); @@ -149,4 +146,4 @@ class OccupancyGridBasedCollisionDetector } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_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 c270a1500431c..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 @@ -17,8 +17,6 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 4d1f1fdb959c0..26c0f247eb558 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -18,15 +18,10 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" -#include -#include - #include #include #include -#include -#include -#include +#include #include @@ -40,7 +35,6 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; struct ParallelParkingParameters { diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp index 1d4245bfec0e4..e1f3402e8dfdf 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -19,8 +19,6 @@ #include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include #include @@ -72,7 +70,7 @@ std::pair getPairsTerminalVelocityAndAccel( std::optional generateFeasibleStopPath( PathWithLaneId & current_path, std::shared_ptr planner_data, - geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + std::optional & stop_pose, const double maximum_deceleration, const double maximum_jerk); /** diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index f66f425f1b7fa..e33c2596ab04d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -23,8 +23,6 @@ #include #include -#include -#include #include #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index fcb4f3f4dc207..13009d5114439 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -26,14 +26,6 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 099adc21d2ad7..165a11ebb54b4 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -15,11 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#include +#include +#include #include #include +#include #include #include #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp index 4ac6f01da65fb..f109bb2cbb213 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp @@ -58,8 +58,6 @@ PathWithLaneId resamplePathWithSpline( const PathWithLaneId & path, const double interval, const bool keep_input_points = false, const std::pair target_section = {0.0, std::numeric_limits::max()}); -Path toPath(const PathWithLaneId & input); - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index bf48252a01dcd..62fd87bbd91bc 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -37,43 +37,6 @@ using autoware_perception_msgs::msg::TrafficSignal; using autoware_perception_msgs::msg::TrafficSignalElement; using geometry_msgs::msg::Pose; -/** - * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. - * - * Iterates through the traffic light elements to find a circle-shaped light that matches the given - * color. - * - * @param tl_state The traffic light state to check. - * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. - * @return True if a circle-shaped light with the specified color is found, false otherwise. - */ -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color); - -/** - * @brief Checks if a traffic light state includes a light with the specified shape. - * - * Searches through the traffic light elements to find a light that matches the given shape. - * - * @param tl_state The traffic light state to check. - * @param shape The shape to look for in the traffic light's lights. - * @return True if a light with the specified shape is found, false otherwise. - */ -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape); - -/** - * @brief Determines if a traffic signal indicates a stop for the given lanelet. - * - * Evaluates the current state of the traffic light, considering if it's green or unknown, - * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet - * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can - * proceed based on allowed turn directions. - * - * @param lanelet The lanelet to check for a stop signal at its traffic light. - * @param tl_state The current state of the traffic light associated with the lanelet. - * @return True if the traffic signal indicates a stop is required, false otherwise. - */ -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state); - /** * @brief Computes the distance from the current position to the next traffic light along a set of * lanelets. @@ -129,6 +92,21 @@ bool isStoppedAtRedTrafficLightWithinDistance( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const double distance_threshold = std::numeric_limits::infinity()); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param planner_data Shared pointer to the planner data containing vehicle state information. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::traffic_light #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ 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 3a7d346849e16..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 @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ #include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -34,18 +33,10 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif #include #include #include -#include #include namespace behavior_path_planner::utils @@ -55,34 +46,21 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; -using path_safety_checker::ExtendedPredictedObject; -using path_safety_checker::ObjectTypesToCheck; -using path_safety_checker::PoseWithVelocityAndPolygonStamped; -using path_safety_checker::PoseWithVelocityStamped; -using path_safety_checker::PredictedPathWithPolygon; -using path_safety_checker::SafetyCheckParams; -using path_safety_checker::TargetObjectsOnLane; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; struct PolygonPoint { geometry_msgs::msg::Point point; - size_t bound_seg_idx; - double lon_dist_to_segment; - double lat_dist_to_bound; + size_t bound_seg_idx{0}; + double lon_dist_to_segment{0.0}; + double lat_dist_to_bound{0.0}; bool is_after(const PolygonPoint & other_point) const { @@ -243,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( @@ -325,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, @@ -333,10 +318,6 @@ lanelet::ConstLanelets calcLaneAroundPose( bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); -double calcMinimumLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double backward_buffer, const double length_to_intersection = 0.0); - lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index b289ce86ca60e..db5dc51150e55 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -11,6 +11,8 @@ Satoshi Ota + Mamoru Sobue + Daniel Sanchez Maxime CLEMENT Takayuki Murooka Satoshi Ota @@ -48,8 +50,6 @@ geometry_msgs interpolation lanelet2_extension - libboost-dev - libopencv-dev magic_enum motion_utils object_recognition_utils @@ -57,13 +57,10 @@ rclcpp route_handler rtc_interface - sensor_msgs tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros tier4_autoware_utils tier4_planning_msgs + traffic_light_utils vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp b/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp index 93ae7e8289902..cbced86865a99 100644 --- a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp +++ b/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp @@ -34,12 +34,12 @@ void SteeringFactorInterface::publishSteeringFactor(const rclcpp::Time & stamp) void SteeringFactorInterface::updateSteeringFactor( const std::array & pose, const std::array distance, const std::string & behavior, const uint16_t direction, const uint16_t status, - const std::string detail) + const std::string & detail) { std::lock_guard lock(mutex_); SteeringFactor factor; factor.pose = pose; - std::array converted_distance; + std::array converted_distance{0.0, 0.0}; for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast(distance[i]); factor.distance = converted_distance; factor.behavior = behavior; diff --git a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp index 4dc422a81d22c..d97ea2e09a89d 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp @@ -30,9 +30,7 @@ namespace marker_utils { -using behavior_path_planner::ShiftLine; using behavior_path_planner::utils::calcPathArcLengthArray; -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; @@ -42,6 +40,61 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + marker.points.push_back(marker.points.front()); +} + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + + addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) @@ -101,7 +154,7 @@ MarkerArray createShiftLineMarkerArray( { ShiftLineArray shift_lines_local = shift_lines; if (shift_lines.empty()) { - shift_lines_local.push_back(ShiftLine()); + shift_lines_local.emplace_back(); } MarkerArray msg; @@ -238,90 +291,6 @@ MarkerArray createLaneletsAreaMarkerArray( return msg; } -MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings) -{ - if (linestrings.empty()) { - return MarkerArray(); - } - - Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shared_linestring_lanelets", 0L, Marker::LINE_STRIP, - createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(0.996, 0.658, 0.466, 0.999)); - marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - - const auto reserve_size = linestrings.size() / 2; - lanelet::ConstLineStrings3d lefts; - lanelet::ConstLineStrings3d rights; - lefts.reserve(reserve_size); - rights.reserve(reserve_size); - - size_t total_marker_reserve_size{0}; - for (size_t idx = 1; idx < linestrings.size(); idx += 2) { - rights.emplace_back(linestrings.at(idx - 1)); - lefts.emplace_back(linestrings.at(idx)); - - for (const auto & ls : linestrings.at(idx - 1).basicLineString()) { - total_marker_reserve_size += ls.size(); - } - for (const auto & ls : linestrings.at(idx).basicLineString()) { - total_marker_reserve_size += ls.size(); - } - } - - if (!total_marker_reserve_size) { - marker.points.reserve(total_marker_reserve_size); - } - - const auto & first_ls = lefts.front().basicLineString(); - for (const auto & ls : first_ls) { - marker.points.push_back(createPoint(ls.x(), ls.y(), ls.z())); - } - - for (auto idx = lefts.cbegin() + 1; idx != lefts.cend(); ++idx) { - Point front = createPoint( - idx->basicLineString().front().x(), idx->basicLineString().front().y(), - idx->basicLineString().front().z()); - Point front_inverted = createPoint( - idx->invert().basicLineString().front().x(), idx->invert().basicLineString().front().y(), - idx->invert().basicLineString().front().z()); - - const auto & marker_back = marker.points.back(); - const bool isFrontNear = tier4_autoware_utils::calcDistance2d(marker_back, front) < - tier4_autoware_utils::calcDistance2d(marker_back, front_inverted); - const auto & left_ls = (isFrontNear) ? idx->basicLineString() : idx->invert().basicLineString(); - for (auto ls = left_ls.cbegin(); ls != left_ls.cend(); ++ls) { - marker.points.push_back(createPoint(ls->x(), ls->y(), ls->z())); - } - } - - for (auto idx = rights.crbegin(); idx != rights.crend(); ++idx) { - Point front = createPoint( - idx->basicLineString().front().x(), idx->basicLineString().front().y(), - idx->basicLineString().front().z()); - Point front_inverted = createPoint( - idx->invert().basicLineString().front().x(), idx->invert().basicLineString().front().y(), - idx->invert().basicLineString().front().z()); - - const auto & marker_back = marker.points.back(); - const bool isFrontFurther = tier4_autoware_utils::calcDistance2d(marker_back, front) > - tier4_autoware_utils::calcDistance2d(marker_back, front_inverted); - const auto & right_ls = - (isFrontFurther) ? idx->basicLineString() : idx->invert().basicLineString(); - for (auto ls = right_ls.crbegin(); ls != right_ls.crend(); ++ls) { - marker.points.push_back(createPoint(ls->x(), ls->y(), ls->z())); - } - } - - if (!marker.points.empty()) { - marker.points.push_back(marker.points.front()); - } - - MarkerArray msg; - - msg.markers.push_back(marker); - return msg; -} - MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, const float & g, const float & b, const float & w) @@ -433,7 +402,6 @@ MarkerArray createPredictedPathMarkerArray( Marker marker = createDefaultMarker( "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), - createMarkerColor(r, g, b, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); @@ -443,34 +411,11 @@ MarkerArray createPredictedPathMarkerArray( marker.points.clear(); const auto & predicted_path_pose = path.at(i); - const double half_width = vehicle_info.vehicle_width_m / 2.0; - const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; - const double base_to_rear = vehicle_info.rear_overhang_m; - - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0) - .position); - marker.points.push_back(marker.points.front()); + addFootprintMarker(marker, predicted_path_pose, vehicle_info); marker_array.markers.push_back(marker); } return marker_array; - - marker.points.reserve(path.size()); - for (const auto & point : path) { - marker.points.push_back(point.position); - } - msg.markers.push_back(marker); - return msg; } MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) 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 933330dafae7e..6ddc0df4eebf4 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -266,7 +266,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( turn_signal_info.required_start_point = lane_front_pose; turn_signal_info.required_end_point = get_required_end_point(combined_lane.centerline3d()); turn_signal_info.desired_end_point = lane_back_pose; - turn_signal_info.turn_signal.command = signal_map.at(lane_attribute); + turn_signal_info.turn_signal.command = g_signal_map.at(lane_attribute); signal_queue.push(turn_signal_info); } } @@ -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/drivable_area_expansion.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 4fe9a3cd6e10f..da7f0fbb327d7 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -20,14 +20,13 @@ #include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include #include #include #include #include #include -#include +#include #include @@ -121,52 +120,78 @@ double calculate_minimum_lane_width( return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } -std::vector calculate_minimum_expansions( - const std::vector & path_poses, const std::vector bound, - const std::vector curvatures, const Side side, - const DrivableAreaExpansionParameters & params) +void calculate_bound_index_mappings( + Expansion & expansion, const std::vector & path_poses, const std::vector & bound, + const Side side) { - std::vector minimum_expansions(bound.size()); size_t lb_idx = 0; + auto & bound_indexes = + (side == LEFT ? expansion.left_bound_indexes : expansion.right_bound_indexes); + auto & bound_projections = + (side == LEFT ? expansion.left_projections : expansion.right_projections); + bound_indexes.resize(path_poses.size(), 0LU); + bound_projections.resize(path_poses.size(), {{}, std::numeric_limits::max()}); for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { - const auto & path_pose = path_poses[path_idx]; - if (curvatures[path_idx] == 0.0) continue; - const auto curvature_radius = 1 / curvatures[path_idx]; - const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); - const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? 1.0 : -1.0); - const auto offset_point = - tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; + const auto path_p = convert_point(path_poses[path_idx].position); for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { - const auto & prev_p = bound[bound_idx]; - const auto & next_p = bound[bound_idx + 1]; - const auto intersection_point = - tier4_autoware_utils::intersect(offset_point, path_pose.position, prev_p, next_p); - if (intersection_point) { - lb_idx = bound_idx; - const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); - minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); - minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); - // apply the expansion to all bound points within the extra arc length - auto arc_length = - tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); - for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { - arc_length += - tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]); - if (arc_length > params.extra_arc_length) break; - minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); - } - arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]); - for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { - const auto idx = bound_idx - down_offset_idx; - arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); - if (arc_length > params.extra_arc_length) break; - minimum_expansions[idx] = std::max(minimum_expansions[idx], dist); - } - break; + const auto prev_p = convert_point(bound[bound_idx]); + const auto next_p = convert_point(bound[bound_idx + 1]); + const auto projection = point_to_segment_projection(path_p, prev_p, next_p); + if (projection.distance < bound_projections[path_idx].distance) { + bound_indexes[path_idx] = bound_idx; + bound_projections[path_idx] = projection; } } + lb_idx = bound_indexes[path_idx]; + } +} + +void apply_arc_length_range_smoothing( + Expansion & expansion, const std::vector & bound, const double arc_length_range, + const Side side) +{ + const auto & bound_indexes = + side == LEFT ? expansion.left_bound_indexes : expansion.right_bound_indexes; + const auto & bound_projections = + side == LEFT ? expansion.left_projections : expansion.right_projections; + auto & bound_expansions = side == LEFT ? expansion.left_distances : expansion.right_distances; + const auto original_expansions = bound_expansions; + for (auto path_idx = 0UL; path_idx < bound_indexes.size(); ++path_idx) { + const auto bound_idx = bound_indexes[path_idx]; + auto arc_length = boost::geometry::distance( + bound_projections[path_idx].point, convert_point(bound[bound_idx + 1])); + const auto update_arc_length_and_bound_expansions = [&](auto idx) { + arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + bound_expansions[idx] = std::max(bound_expansions[idx], original_expansions[bound_idx]); + }; + for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { + update_arc_length_and_bound_expansions(up_bound_idx); + if (arc_length > arc_length_range) break; + } + arc_length = + boost::geometry::distance(bound_projections[path_idx].point, convert_point(bound[bound_idx])); + for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { + update_arc_length_and_bound_expansions(bound_idx - down_offset_idx); + if (arc_length > arc_length_range) break; + } } - return minimum_expansions; +} + +Expansion calculate_expansion( + const std::vector & path_poses, const std::vector & left_bound, + const std::vector & right_bound, const std::vector & curvatures, + const DrivableAreaExpansionParameters & params) +{ + Expansion expansion; + expansion.min_lane_widths.resize(path_poses.size(), 0.0); + for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { + if (curvatures[path_idx] == 0.0) continue; + const auto curvature_radius = 1 / curvatures[path_idx]; + expansion.min_lane_widths[path_idx] = calculate_minimum_lane_width(curvature_radius, params); + } + calculate_bound_index_mappings(expansion, path_poses, left_bound, LEFT); + calculate_bound_index_mappings(expansion, path_poses, right_bound, RIGHT); + return expansion; } void apply_bound_change_rate_limit( @@ -185,21 +210,30 @@ void apply_bound_change_rate_limit( } std::vector calculate_maximum_distance( - const std::vector & path_poses, const std::vector bound, - const SegmentRtree & uncrossable_segments, const std::vector & uncrossable_polygons, - const DrivableAreaExpansionParameters & params) + const std::vector & bound, const SegmentRtree & uncrossable_segments, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params, const Side side) { - // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) std::vector maximum_distances(bound.size(), std::numeric_limits::max()); - LineString2d path_ls; LineString2d bound_ls; for (const auto & p : bound) bound_ls.push_back(convert_point(p)); - for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { const Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + const auto segment_vector = segment_ls.second - segment_ls.first; + const auto is_point_on_correct_side = [&](const Point2d & p) { + const auto point_vector = p - segment_ls.first; + const auto cross_product = + (segment_vector.x() * point_vector.y() - segment_vector.y() * point_vector.x()); + return cross_product * (side == LEFT ? -1.0 : 1.0) <= 0.0; + }; + const auto is_on_correct_side = [&](const Segment2d & segment) { + return is_point_on_correct_side(segment.first) || is_point_on_correct_side(segment.second); + }; std::vector query_result; boost::geometry::index::query( - uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1), + uncrossable_segments, + boost::geometry::index::nearest(segment_ls, 1) && + boost::geometry::index::satisfies(is_on_correct_side), std::back_inserter(query_result)); if (!query_result.empty()) { const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front()); @@ -208,9 +242,18 @@ std::vector calculate_maximum_distance( maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); } for (const auto & uncrossable_poly : uncrossable_polygons) { - const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); - maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); - maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + if (boost::geometry::intersects(uncrossable_poly.outer(), segment_ls)) { + maximum_distances[i] = 0.0; + maximum_distances[i + 1] = 0.0; + break; + } + if (std::all_of( + uncrossable_poly.outer().begin(), uncrossable_poly.outer().end(), + is_point_on_correct_side)) { + const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + } } } if (params.max_expansion_distance > 0.0) @@ -228,8 +271,7 @@ void expand_bound( if (expansions[idx] > 0.0) { const auto bound_p = convert_point(bound[idx]); const auto projection = point_to_linestring_projection(bound_p, path_ls); - const auto expansion_ratio = - (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + const auto expansion_ratio = (expansions[idx] + projection.distance) / projection.distance; const auto & path_p = projection.projected_point; const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); bound[idx].x = expanded_p.x(); @@ -271,6 +313,49 @@ std::vector calculate_smoothed_curvatures( } return smoothed_curvatures; } +void calculate_expansion_distances( + Expansion & expansion, const std::vector & max_left_expansions, + const std::vector & max_right_expansions) +{ + expansion.left_distances.resize(max_left_expansions.size(), 0.0); + expansion.right_distances.resize(max_right_expansions.size(), 0.0); + for (auto path_idx = 0UL; path_idx < expansion.min_lane_widths.size(); ++path_idx) { + const auto left_bound_idx = expansion.left_bound_indexes[path_idx]; + const auto right_bound_idx = expansion.right_bound_indexes[path_idx]; + const auto original_width = expansion.left_projections[path_idx].distance + + expansion.right_projections[path_idx].distance; + const auto expansion_dist = expansion.min_lane_widths[path_idx] - original_width; + if (expansion_dist <= 0.0) continue; + const auto left_limit = + std::min(max_left_expansions[left_bound_idx], max_left_expansions[left_bound_idx + 1]); + const auto right_limit = + std::min(max_right_expansions[right_bound_idx], max_right_expansions[right_bound_idx + 1]); + const auto expansion_fits_on_left_side = expansion_dist / 2.0 < left_limit; + const auto expansion_fits_on_right_side = expansion_dist / 2.0 < right_limit; + if (expansion_fits_on_left_side && expansion_fits_on_right_side) { + expansion.left_distances[left_bound_idx] = expansion_dist / 2.0; + expansion.left_distances[left_bound_idx + 1] = expansion_dist / 2.0; + expansion.right_distances[right_bound_idx] = expansion_dist / 2.0; + expansion.right_distances[right_bound_idx + 1] = expansion_dist / 2.0; + } else if (!expansion_fits_on_left_side) { + expansion.left_distances[left_bound_idx] = left_limit; + expansion.left_distances[left_bound_idx + 1] = left_limit; + const auto compensation_dist = expansion_dist / 2.0 - left_limit; + expansion.right_distances[right_bound_idx] = + std::min(right_limit, expansion_dist / 2.0 + compensation_dist); + expansion.right_distances[right_bound_idx + 1] = + std::min(right_limit, expansion_dist / 2.0 + compensation_dist); + } else if (!expansion_fits_on_right_side) { + expansion.right_distances[right_bound_idx] = right_limit; + expansion.right_distances[right_bound_idx + 1] = right_limit; + const auto compensation_dist = expansion_dist / 2.0 - right_limit; + expansion.left_distances[left_bound_idx] = + std::min(left_limit, expansion_dist / 2.0 + compensation_dist); + expansion.left_distances[left_bound_idx + 1] = + std::min(left_limit, expansion_dist / 2.0 + compensation_dist); + } + } +} void expand_drivable_area( PathWithLaneId & path, @@ -281,7 +366,6 @@ void expand_drivable_area( tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("overall"); stop_watch.tic("preprocessing"); - // crop first/last non deviating path_poses const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; const auto uncrossable_segments = extract_uncrossable_segments( @@ -304,32 +388,34 @@ void expand_drivable_area( const auto first_new_point_idx = curvatures.size(); curvatures.insert( curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); - auto left_expansions = - calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); - auto right_expansions = - calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); + auto expansion = + calculate_expansion(path_poses, path.left_bound, path.right_bound, curvatures, params); const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion"); stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance( - path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params); + path.left_bound, uncrossable_segments, uncrossable_polygons, params, LEFT); const auto max_right_expansions = calculate_maximum_distance( - path_poses, path.right_bound, uncrossable_segments, uncrossable_polygons, params); - for (auto i = 0LU; i < left_expansions.size(); ++i) - left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); - for (auto i = 0LU; i < right_expansions.size(); ++i) - right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]); + path.right_bound, uncrossable_segments, uncrossable_polygons, params, RIGHT); const auto max_dist_ms = stop_watch.toc("max_dist"); + calculate_expansion_distances(expansion, max_left_expansions, max_right_expansions); + apply_arc_length_range_smoothing(expansion, path.left_bound, params.arc_length_range, LEFT); + apply_arc_length_range_smoothing(expansion, path.right_bound, params.arc_length_range, RIGHT); + stop_watch.tic("smooth"); - apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); - apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); + apply_bound_change_rate_limit(expansion.left_distances, path.left_bound, params.max_bound_rate); + apply_bound_change_rate_limit(expansion.right_distances, path.right_bound, params.max_bound_rate); const auto smooth_ms = stop_watch.toc("smooth"); + // reapply expansion limits that may have been broken by the previous smoothing + for (auto i = 0LU; i < expansion.left_distances.size(); ++i) + expansion.left_distances[i] = std::min(expansion.left_distances[i], max_left_expansions[i]); + for (auto i = 0LU; i < expansion.right_distances.size(); ++i) + expansion.right_distances[i] = std::min(expansion.right_distances[i], max_right_expansions[i]); - // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) stop_watch.tic("expand"); - expand_bound(path.left_bound, path_poses, left_expansions); - expand_bound(path.right_bound, path_poses, right_expansions); + expand_bound(path.left_bound, path_poses, expansion.left_distances); + expand_bound(path.right_bound, path_poses, expansion.right_distances); const auto expand_ms = stop_watch.toc("expand"); const auto total_ms = stop_watch.toc("overall"); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 6b323be6c6328..787af681bf69b 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -19,10 +19,7 @@ #include #include -#include - -#include -#include +#include #include diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp index 2eb55599b83f9..8420ee96b0ba6 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp @@ -14,7 +14,7 @@ #include "behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" -#include +#include #include #include 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 f6344b94a6bc5..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 @@ -35,19 +35,22 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); - const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx), target_point); + return calcDistance2d(points.at(seg_idx), target_point); } if (segment_length < lon_dist) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx + 1), target_point); + return calcDistance2d(points.at(seg_idx + 1), target_point); } return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); }(); @@ -64,6 +67,49 @@ size_t findNearestSegmentIndexFromLateralDistance( return motion_utils::findNearestSegmentIndex(points, target_point); } +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Pose & target_point, + const double yaw_threshold) +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + + std::optional closest_idx{std::nullopt}; + double min_lateral_dist = std::numeric_limits::max(); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const auto base_yaw = tf2::getYaw(target_point.orientation); + const auto yaw = + normalizeRadian(calcAzimuthAngle(points.at(seg_idx), points.at(seg_idx + 1)) - base_yaw); + if (yaw_threshold < std::abs(yaw)) { + continue; + } + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point.position); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return calcDistance2d(points.at(seg_idx), target_point.position); + } + if (segment_length < lon_dist) { + return calcDistance2d(points.at(seg_idx + 1), target_point.position); + } + return std::abs(motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); + }(); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; + } + } + + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point.position); +} + bool checkHasSameLane( const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelet & target_lane) { @@ -102,6 +148,40 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } + +std::vector extractBoundFromPolygon( + const lanelet::ConstPolygon3d & polygon, const size_t start_idx, const size_t end_idx, + const bool clockwise) +{ + std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { + ret.push_back(polygon[i]); + + if (i + 1 == polygon.size() && clockwise) { + if (end_idx == 0) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = 0; + ret.push_back(polygon[i]); + continue; + } + + if (i == 0 && !clockwise) { + if (end_idx == polygon.size() - 1) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = polygon.size() - 1; + ret.push_back(polygon[i]); + continue; + } + } + + ret.push_back(polygon[end_idx]); + + return ret; +} } // namespace namespace behavior_path_planner::utils::drivable_area_processing @@ -128,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( @@ -158,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); } @@ -586,48 +665,44 @@ std::vector cutOverlappedLanes( [&has_same_id_lane, &p](const auto & lanelet) { return has_same_id_lane(lanelet, p); }); }; - // Step1. find first path point within drivable lanes - size_t start_point_idx = std::numeric_limits::max(); - for (const auto & lanes : shorten_lanes) { - for (size_t i = 0; i < original_points.size(); ++i) { - // check right lane - if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + const auto is_point_in_drivable_lanes = [&has_same_id_lane, &has_same_id_lanes]( + const auto & lanes, const auto & p) { + if (has_same_id_lane(lanes.right_lane, p)) { + return true; + } + // check left lane + if (has_same_id_lane(lanes.left_lane, p)) { + return true; + } + // check middle lanes + if (has_same_id_lanes(lanes.middle_lanes, p)) { + return true; + } + return false; + }; - // check left lane - if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + // Step1. find first path point within drivable lanes + size_t start_point_idx = original_points.size(); - // check middle lanes - if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { - start_point_idx = std::min(start_point_idx, i); - } + for (size_t i = 0; i < original_points.size(); ++i) { + const bool first_path_point_in_drivable_lane_found = std::any_of( + shorten_lanes.begin(), shorten_lanes.end(), + [&is_point_in_drivable_lanes, &original_points, i](const auto & lanes) { + return is_point_in_drivable_lanes(lanes, original_points.at(i)); + }); + if (first_path_point_in_drivable_lane_found) { + start_point_idx = i; + break; } } // Step2. pick up only path points within drivable lanes for (const auto & lanes : shorten_lanes) { for (size_t i = start_point_idx; i < original_points.size(); ++i) { - // check right lane - if (has_same_id_lane(lanes.right_lane, original_points.at(i))) { + if (is_point_in_drivable_lanes(lanes, original_points.at(i))) { path.points.push_back(original_points.at(i)); continue; } - - // check left lane - if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { - path.points.push_back(original_points.at(i)); - continue; - } - - // check middle lanes - if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { - path.points.push_back(original_points.at(i)); - continue; - } - start_point_idx = i; break; } @@ -799,9 +874,9 @@ void generateDrivableArea( const auto front_pose = cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose, M_PI_2); const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose, M_PI_2); const auto left_start_point = calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); const auto right_start_point = calcLongitudinalOffsetStartPoint( @@ -821,9 +896,9 @@ void generateDrivableArea( // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose, M_PI_2); const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position); + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose, M_PI_2); const auto left_goal_point = calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); const auto right_goal_point = @@ -1012,34 +1087,6 @@ void generateDrivableArea( path.right_bound = modified_right_bound; } -// TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if -// we can refactor some of the code for better readability -lanelet::ConstLineStrings3d getMaximumDrivableArea( - const std::shared_ptr & planner_data) -{ - const auto & p = planner_data->parameters; - const auto & route_handler = planner_data->route_handler; - const auto & ego_pose = planner_data->self_odometry->pose.pose; - - lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "failed to find closest lanelet within route!!!"); - return {}; - } - - const auto current_lanes = route_handler->getLaneletSequence( - current_lane, ego_pose, p.backward_path_length, p.forward_path_length); - lanelet::ConstLineStrings3d linestring_shared; - for (const auto & lane : current_lanes) { - lanelet::ConstLineStrings3d furthest_line = route_handler->getFurthestLinestring(lane); - linestring_shared.insert(linestring_shared.end(), furthest_line.begin(), furthest_line.end()); - } - - return linestring_shared; -} - std::vector expandLanelets( const std::vector & drivable_lanes, const double left_bound_offset, const double right_bound_offset, const std::vector & types_to_skip) @@ -1089,13 +1136,17 @@ void extractObstaclesFromDrivableArea( std::vector> right_polygons; std::vector> left_polygons; for (const auto & obstacle : obstacles) { + if (obstacle.poly.outer().empty()) { + continue; + } + const auto & obj_pos = obstacle.pose.position; // get edge points of the object const size_t nearest_path_idx = motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon std::vector edge_points; - for (size_t i = 0; i < obstacle.poly.outer().size() - 1; + for (int i = 0; i < static_cast(obstacle.poly.outer().size()) - 1; ++i) { // NOTE: There is a duplicated points edge_points.push_back(tier4_autoware_utils::createPoint( obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(), @@ -1183,6 +1234,8 @@ std::vector getBoundWithHatchedRoadMarkings( } else { if (!polygon) { will_close_polygon = true; + } else if (polygon.value().id() != current_polygon.value().id()) { + will_close_polygon = true; } else { current_polygon_border_indices.push_back( get_corresponding_polygon_index(*current_polygon, bound_point.id())); @@ -1217,6 +1270,17 @@ std::vector getBoundWithHatchedRoadMarkings( (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); } } + + if (polygon.has_value() && current_polygon.has_value()) { + if (polygon.value().id() != current_polygon.value().id()) { + current_polygon = polygon; + current_polygon_border_indices.clear(); + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(current_polygon.value(), bound_point.id())); + continue; + } + } + current_polygon = std::nullopt; current_polygon_border_indices.clear(); } @@ -1230,36 +1294,6 @@ std::vector getBoundWithIntersectionAreas( const std::shared_ptr & route_handler, const std::vector & drivable_lanes, const bool is_left) { - const auto extract_bound_from_polygon = - [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret{}; - for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(polygon[i]); - - if (i + 1 == polygon.size() && clockwise) { - if (end_idx == 0) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = 0; - continue; - } - - if (i == 0 && !clockwise) { - if (end_idx == polygon.size() - 1) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = polygon.size() - 1; - continue; - } - } - - ret.push_back(polygon[end_idx]); - - return ret; - }; - std::vector expanded_bound = original_bound; // expand drivable area by using intersection area. @@ -1305,7 +1339,7 @@ std::vector getBoundWithIntersectionAreas( const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); intersection_bound = - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); + extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); } // Step2. check shared bound point. diff --git a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp rename to planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 0f87c68e38289..1aab58ac2ff6e 100644 --- a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 7ba4f114328a7..a1a079e679e72 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -23,38 +23,22 @@ #include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include +#include #include -#endif - -#include +#include #include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::Transform; -using geometry_msgs::msg::TransformStamped; using lanelet::utils::getArcCoordinates; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::deg2rad; using tier4_autoware_utils::inverseTransformPoint; -using tier4_autoware_utils::inverseTransformPose; using tier4_autoware_utils::normalizeRadian; -using tier4_autoware_utils::toMsg; using tier4_autoware_utils::transformPose; namespace behavior_path_planner @@ -518,15 +502,11 @@ std::vector GeometricParallelParking::planOneTrial( // set terminal velocity and acceleration(temporary implementation) if (is_forward) { - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.forward_parking_velocity, 0.0)); - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.forward_parking_velocity, 0.0); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.forward_parking_velocity, 0.0); } else { - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.backward_parking_velocity, 0.0)); - pairs_terminal_velocity_and_accel_.push_back( - std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.backward_parking_velocity, 0.0); + pairs_terminal_velocity_and_accel_.emplace_back(parameters_.backward_parking_velocity, 0.0); } // set pull_over start and end pose diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp index 58fd392988ad9..97eba861f658a 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/utils.hpp" #include +#include namespace behavior_path_planner::utils::parking_departure { @@ -125,7 +126,7 @@ std::pair getPairsTerminalVelocityAndAccel( std::optional generateFeasibleStopPath( PathWithLaneId & current_path, std::shared_ptr planner_data, - geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + std::optional & stop_pose, const double maximum_deceleration, const double maximum_jerk) { if (current_path.points.empty()) { 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 f5d76f5cd4d35..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); } } } @@ -514,8 +513,6 @@ std::vector getCollidedPolygons( const auto interpolated_data = getInterpolatedPoseWithVelocityAndPolygonStamped( predicted_ego_path, current_time, ego_vehicle_info); if (!interpolated_data) { - debug.expected_obj_pose = obj_pose; - debug.extended_obj_polygon = obj_polygon; continue; } const auto & ego_pose = interpolated_data->pose; diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp index 994a7ba1a19d5..1d51426793de8 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -155,19 +155,6 @@ PathWithLaneId resamplePathWithSpline( return motion_utils::resamplePath(path, s_out); } -Path toPath(const PathWithLaneId & input) -{ - Path output{}; - output.header = input.header; - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - output.points.resize(input.points.size()); - for (size_t i = 0; i < input.points.size(); ++i) { - output.points.at(i) = input.points.at(i).point; - } - return output; -} - size_t getIdxByArclength( const PathWithLaneId & path, const size_t target_idx, const double signed_arc) { @@ -186,16 +173,15 @@ size_t getIdxByArclength( } } return path.points.size() - 1; - } else { - for (size_t i = target_idx; i > 0; --i) { - const auto next_i = i - 1; - sum_length -= calcDistance2d(path.points.at(i), path.points.at(next_i)); - if (sum_length < signed_arc) { - return next_i; - } + } + for (size_t i = target_idx; i > 0; --i) { + const auto next_i = i - 1; + sum_length -= calcDistance2d(path.points.at(i), path.points.at(next_i)); + if (sum_length < signed_arc) { + return next_i; } - return 0; } + return 0; } // TODO(murooka) This function should be replaced with motion_utils::cropPoints @@ -642,8 +628,8 @@ BehaviorModuleOutput getReferencePath( dp.drivable_area_types_to_skip); BehaviorModuleOutput output; - output.path = std::make_shared(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; @@ -692,8 +678,8 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr(reference_path); - output.reference_path = std::make_shared(reference_path); + output.path = reference_path; + output.reference_path = reference_path; output.drivable_area_info.drivable_lanes = drivable_lanes; return output; diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 850919f539e59..ecd02bfd63485 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -14,63 +14,12 @@ #include #include +#include namespace behavior_path_planner::utils::traffic_light { using motion_utils::calcSignedArcLength; -bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) -{ - const auto it_lamp = - std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; - }); - - return it_lamp != tl_state.elements.end(); -} - -bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) -{ - const auto it_lamp = std::find_if( - tl_state.elements.begin(), tl_state.elements.end(), - [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - - return it_lamp != tl_state.elements.end(); -} - -bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) -{ - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { - return false; - } - - if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { - return false; - } - - const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); - - if (turn_direction == "else") { - return true; - } - if ( - turn_direction == "right" && - hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { - return false; - } - if ( - turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { - return false; - } - if ( - turn_direction == "straight" && - hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { - return false; - } - - return true; -} - double getDistanceToNextTrafficLight( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { @@ -138,7 +87,8 @@ std::optional calcDistanceToRedTrafficLight( continue; } - if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + if (!traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { continue; } @@ -177,4 +127,24 @@ bool isStoppedAtRedTrafficLightWithinDistance( return (distance_to_red_traffic_light < distance_threshold); } +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (traffic_light_utils::isTrafficSignalStop( + lanelet, traffic_signal_stamped.value().signal)) { + return true; + } + } + } + + return false; +} + } // namespace behavior_path_planner::utils::traffic_light diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 88c7532b0ad72..308cf9c4fed2c 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" @@ -22,6 +21,7 @@ #include #include #include +#include #include #include @@ -74,7 +74,7 @@ namespace behavior_path_planner::utils using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; -using tf2::fromMsg; +using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; std::optional getPolygonByPoint( @@ -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) @@ -1515,31 +1579,6 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre return true; } -double calcMinimumLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double backward_buffer, const double length_to_intersection) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const double & vel = common_param.minimum_lane_changing_velocity; - const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); - const double & max_lateral_acc = lat_acc.second; - const double & lateral_jerk = common_param.lane_changing_lateral_jerk; - const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - - double accumulated_length = length_to_intersection; - for (const auto & shift_interval : shift_intervals) { - const double t = - PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); - accumulated_length += vel * t + finish_judge_buffer; - } - accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); - - return accumulated_length; -} - lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler) { @@ -1576,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_planner_common/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 2cbaaf46e78cb..77728cc87604d 100644 --- a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -58,7 +58,7 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) Point2d query(5.0, -5.0); Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); - EXPECT_NEAR(projection.distance, -5.0, eps); + EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } @@ -82,7 +82,7 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) Point2d query(0.0, 0.0); Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); - EXPECT_NEAR(projection.distance, -std::sqrt(50), eps); + EXPECT_NEAR(projection.distance, std::sqrt(50), eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } @@ -115,7 +115,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) Point2d query(0.0, 5.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 30.0 + std::sqrt(2.5 * 2.5 * 2), eps); - EXPECT_NEAR(projection.distance, -std::sqrt(2.5 * 2.5 * 2), eps); + EXPECT_NEAR(projection.distance, std::sqrt(2.5 * 2.5 * 2), eps); EXPECT_NEAR(projection.projected_point.x(), 2.5, eps); EXPECT_NEAR(projection.projected_point.y(), 7.5, eps); } diff --git a/planning/behavior_path_side_shift_module/CMakeLists.txt b/planning/behavior_path_side_shift_module/CMakeLists.txt new file mode 100644 index 0000000000000..28009e48a4cc4 --- /dev/null +++ b/planning/behavior_path_side_shift_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_side_shift_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/utils.cpp + src/manager.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design.md b/planning/behavior_path_side_shift_module/README.md similarity index 71% rename from planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design.md rename to planning/behavior_path_side_shift_module/README.md index b76e549eb602f..6a460d8423cbd 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design.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_planner/config/side_shift/side_shift.param.yaml b/planning/behavior_path_side_shift_module/config/side_shift.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/side_shift/side_shift.param.yaml rename to planning/behavior_path_side_shift_module/config/side_shift.param.yaml 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_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp index 6475661f72ac4..c9edd3d6bf6a6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -51,4 +51,4 @@ struct SideShiftDebugData }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp index 53bac3914ce86..ef8a48bc29760 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "behavior_path_side_shift_module/scene.hpp" #include @@ -50,4 +50,4 @@ class SideShiftModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 014a0f403f9ec..7d72afebfb359 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ -#include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_side_shift_module/data_structs.hpp" #include @@ -134,4 +134,4 @@ class SideShiftModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp index 6cdfd84d79075..210dd4e8a8d2a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #include #include @@ -41,4 +41,4 @@ Point transformToGrid( } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_ +#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/behavior_path_side_shift_module/package.xml new file mode 100644 index 0000000000000..5c421b04c0e39 --- /dev/null +++ b/planning/behavior_path_side_shift_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_side_shift_module + 0.1.0 + The behavior_path_side_shift_module package + + Satoshi Ota + Fumiya Watanabe + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + motion_utils + pluginlib + rclcpp + tier4_autoware_utils + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_side_shift_module/plugins.xml b/planning/behavior_path_side_shift_module/plugins.xml new file mode 100644 index 0000000000000..2688e3a17c2b6 --- /dev/null +++ b/planning/behavior_path_side_shift_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_side_shift_module/src/manager.cpp similarity index 97% rename from planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp rename to planning/behavior_path_side_shift_module/src/manager.cpp index b20712ef19179..fa8d579da3d09 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_side_shift_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "behavior_path_side_shift_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_side_shift_module/src/scene.cpp similarity index 96% rename from planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp rename to planning/behavior_path_side_shift_module/src/scene.cpp index 9f77288e608cf..6df8c1ec629c9 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_side_shift_module/src/scene.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" +#include "behavior_path_side_shift_module/scene.hpp" -#include "behavior_path_planner/utils/side_shift/util.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_side_shift_module/utils.hpp" #include @@ -203,17 +203,17 @@ void SideShiftModule::updateData() ? planner_data_->self_odometry->pose.pose : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); if (prev_reference_.points.empty()) { - prev_reference_ = *getPreviousModuleOutput().path; + prev_reference_ = getPreviousModuleOutput().path; } - if (!getPreviousModuleOutput().reference_path) { + if (getPreviousModuleOutput().reference_path.points.empty()) { return; } const auto centerline_path = utils::calcCenterLinePath( planner_data_, reference_pose, longest_dist_to_shift_line, - *getPreviousModuleOutput().reference_path); + getPreviousModuleOutput().reference_path); constexpr double resample_interval = 1.0; - const auto backward_extened_path = extendBackwardLength(*getPreviousModuleOutput().path); + const auto backward_extened_path = extendBackwardLength(getPreviousModuleOutput().path); reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval); path_shifter_.setPath(reference_path_); @@ -286,7 +286,7 @@ BehaviorModuleOutput SideShiftModule::plan() output.reference_path = getPreviousModuleOutput().reference_path; prev_output_ = shifted_path; - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); debug_data_.path_shifter = std::make_shared(path_shifter_); @@ -329,7 +329,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); prev_output_ = shifted_path; @@ -409,7 +409,8 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat { // for new architecture // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be // assigned without combining. - out.path = std::make_shared(output_path); + out.path = output_path; + out.reference_path = getPreviousModuleOutput().reference_path; out.drivable_area_info.drivable_lanes = expanded_lanes; out.drivable_area_info.is_already_expanded = true; } diff --git a/planning/behavior_path_planner/src/utils/side_shift/util.cpp b/planning/behavior_path_side_shift_module/src/utils.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/side_shift/util.cpp rename to planning/behavior_path_side_shift_module/src/utils.cpp index af733722562f8..a674d57597bc7 100644 --- a/planning/behavior_path_planner/src/utils/side_shift/util.cpp +++ b/planning/behavior_path_side_shift_module/src/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/side_shift/util.hpp" +#include "behavior_path_side_shift_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..eb7d1afe27549 --- /dev/null +++ b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,125 @@ +// 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_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_side_shift_module") + + "/config/side_shift.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_start_planner_module/README.md similarity index 78% rename from planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md rename to planning/behavior_path_start_planner_module/README.md index 28a1db649cf04..b26864eb450d5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -2,17 +2,23 @@ ## Purpose / Role -The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and implementing safety checks against dynamic obstacles. (Note: The feature of safety checks against dynamic obstacles is currently a work in progress.) -This module is activated when a new route is received. +The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and stopping in response to dynamic obstacles when a collision is detected. -Use cases are as follows +Use cases include: -- start smoothly from the current ego position to centerline. - ![case1](../image/start_from_road_lane.drawio.svg) - pull out from the side of the road lane to centerline. - ![case2](../image/start_from_road_side.drawio.svg) + +
+ ![case1](images/start_from_road_side.drawio.svg){width=1000} +
pull out from side of the road lane
+
+ - pull out from the shoulder lane to the road lane centerline. - ![case3](../image/start_from_road_shoulder.drawio.svg) + +
+ ![case2](images/start_from_road_shoulder.drawio.svg){width=1000} +
pull out from the shoulder lane
+
## Design @@ -59,18 +65,19 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles @@ -78,14 +85,14 @@ PullOutPath --o PullOutPlannerBase 2. Calculate object's polygon 3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path -![pull_out_collision_check](../image/pull_out_collision_check.drawio.svg) +![pull_out_collision_check](./images/pull_out_collision_check.drawio.svg) ## Safety check with dynamic obstacles ### **Basic concept of safety check against dynamic obstacles** This is based on the concept of RSS. For the logic used, refer to the link below. -See [safety check feature explanation](../docs/behavior_path_planner_safety_check.md) +See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) ### **Collision check performed range** @@ -103,7 +110,7 @@ Since there's a stop at the midpoint during the shift, this becomes the endpoint During backward movement, no safety check is performed. Safety check begins at the point where the backward movement ends. -![collision_check_range](../image/start_planner/collision_check_range.drawio.svg) +![collision_check_range](./images/collision_check_range.drawio.svg) ### **Ego vehicle's velocity planning** @@ -198,7 +205,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral - In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) - Combine this path with center line of road lane -![shift_pull_out](../image/shift_pull_out.drawio.svg) +![shift_pull_out](./images/shift_pull_out.drawio.svg) [shift pull out video](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4) @@ -220,7 +227,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot. See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. -![geometric_pull_out](../image/geometric_pull_out.drawio.svg) +![geometric_pull_out](./images/geometric_pull_out.drawio.svg) [geometric pull out video](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4) @@ -239,10 +246,56 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`). -![pull_out_after_back](../image/pull_out_after_back.drawio.svg) +![pull_out_after_back](./images/pull_out_after_back.drawio.svg) [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) +### **search priority** + +If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. + +During this backward search, different policies can be applied based on `search_priority` parameters: + +Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. +Opting for `short_back_distance` aims to find a location with the least possible backward movement. + +![priority_order](./images/priority_order.drawio.svg) + +`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. + +#### `efficient_path` + +When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 1 | shift_pull_out | +| ... | ... | +| N | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | geometric_pull_out | + +This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. + +#### `short_back_distance` + +For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | shift_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | shift_pull_out | +| N | geometric_pull_out | + +This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. + ### **parameters for backward pull out start point search** | Name | Unit | Type | Description | Default value | @@ -257,7 +310,7 @@ If a safe path cannot be generated from the current position, search backwards f ### **freespace pull out** If the vehicle gets stuck with pull out along lanes, execute freespace pull out. -To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` @@ -276,4 +329,4 @@ To run this feature, you need to set `parking_lot` to the map, `activate_by_scen | end_pose_search_end_distance | [m] | double | distance from ego to the end point of the search for the end point in the freespace_pull_out driving lane | 30.0 | | end_pose_search_interval | [m] | bool | interval to search for the end point in the freespace_pull_out driving lane | 2.0 | -See [freespace_planner](../../freespace_planner/README.md) for other parameters. +See [freespace_planner](../freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 514d61e225ecd..d174b54b9ccbe 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,8 +5,9 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margin: 1.0 + collision_check_margins: [2.0, 1.5, 1.0] collision_check_distance_from_end: 1.0 + collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 diff --git a/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg b/planning/behavior_path_start_planner_module/images/collision_check_range.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg rename to planning/behavior_path_start_planner_module/images/collision_check_range.drawio.svg diff --git a/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg b/planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/geometric_pull_out.drawio.svg rename to planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg new file mode 100644 index 0000000000000..ab48c30b6fdae --- /dev/null +++ b/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg @@ -0,0 +1,319 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+ + (0, shift_pull_out) +
+ + (1, shift_pull_out) +
+ (2, shift_pull_out) +
+ (3, shift_pull_out) +
+ (4, shift_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (0, + geometric_pull_out + ) +
+ (1, + geometric_pull_out + ) +
+ (2, + geometric_pull_out + ) +
+ (3, + geometric_pull_out + ) +
+ (4, + geometric_pull_out + ) +
+ ︙ +
+ (N, + geometric_pull_out + ) + +
+ +
+
+ + +
+
backward_search_resolution
+
+ + +
+
geometric pull out path
+
+ + +
+
start pose candidate
+
+ + +
+
idx: 0
+
+ + +
+
idx: 1
+
+ + +
+
idx: 2
+
+ + +
+
idx: 3
+
+ + +
+
idx: 4
+
+ + +
+
shift pull out path
+
+ + +
+
+ (idx, planner_type):= +
+ (3, geometric_pull_out) +
+
+ + +
+
(3, shift_pull_out)
+
+ + +
+
idx: N
+
+ + +
+
+ + efficient_path +
+ +
+
+ + +
+
+ (0, shift_pull_out) +
+ (0, geometric_pull_out) +
+ (1, shift_pull_out) +
+ (1, geometric_pull_out) +
+ (2, shift_pull_out) +
+ (2, geometric_pull_out) +
+ (3, shift_pull_out) +
+ (3, geometric_pull_out) +
+ (4, shift_pull_out) +
+ (4, geometric_pull_out) +
+ ︙ +
+ (N, shift_pull_out) +
+ (N, geometric_pull_out) + +
+ +
+
+ + +
+
+ + short_back_distance +
+ +
+
+ + +
+
high priority
+
+ + +
+
low priority
+
+ + diff --git a/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg b/planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/pull_out_after_back.drawio.svg rename to planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg diff --git a/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg b/planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg rename to planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg diff --git a/planning/behavior_path_planner/image/shift_pull_out.drawio.svg b/planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/shift_pull_out.drawio.svg rename to planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg diff --git a/planning/behavior_path_planner/image/start_from_road_shoulder.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/start_from_road_shoulder.drawio.svg rename to planning/behavior_path_start_planner_module/images/start_from_road_shoulder.drawio.svg diff --git a/planning/behavior_path_planner/image/start_from_road_side.drawio.svg b/planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/start_from_road_side.drawio.svg rename to planning/behavior_path_start_planner_module/images/start_from_road_side.drawio.svg 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 78% 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 783aace0983ca..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}; @@ -42,8 +62,9 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; - double collision_check_margin{0.0}; + std::vector collision_check_margins{}; double collision_check_distance_from_end{0.0}; + double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; @@ -104,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 9012504ed2e7a..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 @@ -46,6 +46,15 @@ class ShiftPullOut : public PullOutPlannerBase double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); + bool refineShiftedPathToStartPose( + 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: @@ -54,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 a143f34ead649..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 @@ -68,7 +68,7 @@ struct PullOutStatus Pose pull_out_start_pose{}; bool prev_is_safe_dynamic_objects{false}; std::shared_ptr prev_stop_path_after_approval{nullptr}; - bool has_stop_point{false}; + std::optional stop_pose{std::nullopt}; PullOutStatus() {} }; @@ -169,11 +169,12 @@ class StartPlannerModule : public SceneModuleInterface bool isMoving() const; PriorityOrder determinePriorityOrder( - const std::string & search_priority, const size_t candidates_size); + const std::string & search_priority, const size_t start_pose_candidates_num); bool findPullOutPath( - const std::vector & start_pose_candidates, const size_t index, - const std::shared_ptr & planner, const Pose & refined_start_pose, - const Pose & goal_pose); + const Pose & start_pose_candidate, const std::shared_ptr & planner, + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); + + PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); @@ -190,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_; @@ -227,7 +228,9 @@ class StartPlannerModule : public SceneModuleInterface void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, + const double velocity_threshold, const double object_check_backward_distance, + const double object_check_forward_distance) const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; @@ -239,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; @@ -251,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_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_start_planner_module/src/debug.cpp similarity index 57% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp rename to planning/behavior_path_start_planner_module/src/debug.cpp index 98d52b79e2edf..0f95470d86d40 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ b/planning/behavior_path_start_planner_module/src/debug.cpp @@ -1,4 +1,4 @@ -// Copyright 2021-2023 TIER IV, Inc. +// 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. @@ -12,21 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ +#include "behavior_path_start_planner_module/debug.hpp" namespace behavior_path_planner { -struct LaneFollowingParameters +void updateSafetyCheckDebugData( + StartPlannerDebugData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) { - double lane_change_prepare_duration{0.0}; - - // finding closest lanelet - double distance_threshold{0.0}; - double yaw_threshold{0.0}; -}; + 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_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index 7d6dd60398e8e..d9ff155f36e6c 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -63,17 +63,7 @@ std::optional GeometricPullOut::plan(const Pose & start_pose, const // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); - const auto [pull_out_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, arc_path, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { - return {}; - } const double velocity = parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 58d0fbee7921b..8cc0b34082e44 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,9 +43,12 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); - p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); + p.collision_check_margins = + node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); + p.collision_check_margin_from_front_object = + node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out 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 73952ca02f558..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 @@ -46,15 +46,9 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; - const auto & dynamic_objects = planner_data_->dynamic_object; 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); @@ -64,54 +58,23 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return std::nullopt; } - // extract stop objects in pull out lane for collision check - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); - // get safe path for (auto & pull_out_path : pull_out_paths) { auto & shift_path = pull_out_path.partial_paths.front(); // shift path is not separate but only one. - // check lane_departure and collision with path between pull_out_start to pull_out_end - PathWithLaneId path_start_to_end{}; + // check lane_departure with path between pull_out_start to pull_out_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 @@ -125,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)); @@ -139,14 +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)) { - continue; - } - - // check collision - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { + lane_departure_checker_->checkPathWillLeaveLane(drivable_lanes_, path_shift_start_to_end)) { continue; } @@ -158,6 +114,58 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return std::nullopt; } +bool ShiftPullOut::refineShiftedPathToStartPose( + ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, + const double longitudinal_acc, const double lateral_acc) +{ + constexpr double TOLERANCE = 0.01; + constexpr size_t MAX_ITERATION = 100; + + // Lambda to check if change is above tolerance + auto is_within_tolerance = + [](const auto & prev_val, const auto & current_val, const auto & tolerance) { + return std::abs(current_val - prev_val) < tolerance; + }; + + size_t iteration = 0; + while (iteration < MAX_ITERATION) { + const double lateral_offset = + motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position); + + PathShifter path_shifter; + path_shifter.setPath(shifted_path.path); + ShiftLine shift_line{}; + shift_line.start = start_pose; + shift_line.end = end_pose; + shift_line.end_shift_length = lateral_offset; + path_shifter.addShiftLine(shift_line); + path_shifter.setVelocity(0.0); + path_shifter.setLongitudinalAcceleration(longitudinal_acc); + path_shifter.setLateralAccelerationLimit(lateral_acc); + + if (!path_shifter.generate(&shifted_path, false)) { + RCLCPP_WARN( + rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"), + "Failed to generate shifted path"); + return false; + } + + if (is_within_tolerance( + lateral_offset, + motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), + TOLERANCE)) { + return true; + } + + iteration++; + } + + RCLCPP_WARN( + rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"), + "Failed to converge lateral offset until max iteration"); + return false; +} + std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, const Pose & goal_pose) @@ -303,6 +311,8 @@ std::vector ShiftPullOut::calcPullOutPaths( if (!path_shifter.generate(&shifted_path, offset_back)) { continue; } + refineShiftedPathToStartPose( + shifted_path, start_pose, *shift_end_pose_ptr, longitudinal_acc, lateral_acc); // set velocity const size_t pull_out_end_idx = 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 0d3892cde8023..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( @@ -371,11 +380,11 @@ BehaviorModuleOutput StartPlannerModule::plan() incrementPathIndex(); } - if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { + if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.stop_pose) { auto current_path = getCurrentPath(); const auto stop_path = behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); // Insert stop point in the path if needed @@ -384,17 +393,18 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); path = *stop_path; status_.prev_stop_path_after_approval = std::make_shared(path); - status_.has_stop_point = true; + status_.stop_pose = stop_pose_; } else { path = current_path; } - } else if (!isWaitingApproval() && status_.has_stop_point) { + } else if (!isWaitingApproval() && status_.stop_pose) { // Delete stop point if conditions are met if (status_.is_safe_dynamic_objects && isStopped()) { - status_.has_stop_point = false; + status_.stop_pose = std::nullopt; path = getCurrentPath(); } path = *status_.prev_stop_path_after_approval; + stop_pose_ = status_.stop_pose; } else { path = getCurrentPath(); } @@ -402,11 +412,11 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - output.path = std::make_shared(path); + output.path = path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -512,11 +522,11 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - output.path = std::make_shared(stop_path); + output.path = stop_path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_unique(getPreviousModuleOutput().reference_path); setDrivableAreaInfo(output); @@ -559,6 +569,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; + updateDrivableLanes(); } void StartPlannerModule::incrementPathIndex() @@ -584,26 +595,33 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & [index, planner] : order_priority) { - if (findPullOutPath(start_pose_candidates, index, planner, refined_start_pose, goal_pose)) - return; + for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath( + start_pose_candidates[index], planner, refined_start_pose, goal_pose, + collision_check_margin)) { + debug_data_.selected_start_pose_candidate_index = index; + debug_data_.margin_for_start_pose_candidate = collision_check_margin; + return; + } + } } updateStatusIfNoSafePathFound(); } PriorityOrder StartPlannerModule::determinePriorityOrder( - const std::string & search_priority, const size_t candidates_size) + const std::string & search_priority, const size_t start_pose_candidates_num) { PriorityOrder order_priority; if (search_priority == "efficient_path") { for (const auto & planner : start_planners_) { - for (size_t i = 0; i < candidates_size; i++) { + for (size_t i = 0; i < start_pose_candidates_num; i++) { order_priority.emplace_back(i, planner); } } } else if (search_priority == "short_back_distance") { - for (size_t i = 0; i < candidates_size; i++) { + for (size_t i = 0; i < start_pose_candidates_num; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } @@ -616,46 +634,74 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( } bool StartPlannerModule::findPullOutPath( - const std::vector & start_pose_candidates, const size_t index, - const std::shared_ptr & planner, const Pose & refined_start_pose, - const Pose & goal_pose) + const Pose & start_pose_candidate, const std::shared_ptr & planner, + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin) { - // Ensure the index is within the bounds of the start_pose_candidates vector - if (index >= start_pose_candidates.size()) return false; + 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 = 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); + const auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); - const Pose & pull_out_start_pose = start_pose_candidates.at(index); - const bool is_driving_forward = - tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + // if start_pose_candidate is far from refined_start_pose, backward driving is necessary + const bool backward_is_unnecessary = + tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01; planner->setPlannerData(planner_data_); - const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose); // If no path is found, return false if (!pull_out_path) { return false; } - // If driving forward, update status with the current path and return true - if (is_driving_forward) { - updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner->getPlannerType()); - return true; + // check collision + if (utils::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, + collision_check_margin)) { + return false; } - // If this is the last start pose candidate, return false - if (index == start_pose_candidates.size() - 1) return false; - - const Pose & next_pull_out_start_pose = start_pose_candidates.at(index + 1); - const auto next_pull_out_path = planner->plan(next_pull_out_start_pose, goal_pose); + if (backward_is_unnecessary) { + updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); + return true; + } - // If no next path is found, return false - if (!next_pull_out_path) return false; + updateStatusWithNextPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); - // Update status with the next path and return true - updateStatusWithNextPath( - *next_pull_out_path, next_pull_out_start_pose, planner->getPlannerType()); return true; } +PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) +{ + PathWithLaneId combined_path; + for (const auto & partial_path : path.partial_paths) { + combined_path.points.insert( + combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); + } + // calculate collision check end idx + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + + if (collision_check_end_pose) { + return motion_utils::findNearestIndex( + combined_path.points, collision_check_end_pose->position); + } else { + return combined_path.points.size() - 1; + } + }); + // remove the point behind of collision check end pose + combined_path.points.erase( + combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); + return combined_path; +} + void StartPlannerModule::updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type) @@ -775,13 +821,9 @@ std::vector StartPlannerModule::generateDrivableLanes( void StartPlannerModule::updatePullOutStatus() { - const bool has_received_new_route = - !planner_data_->prev_route_id || - *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route) { + if (!receivedNewRoute()) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -812,9 +854,13 @@ void StartPlannerModule::updatePullOutStatus() return {*refined_start_pose}; }); - planWithPriority( - start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + if (!status_.backward_driving_complete) { + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + } + 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); @@ -871,12 +917,19 @@ 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 = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + + const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, + backward_path_length, std::numeric_limits::max()); - const auto stop_objects_in_pull_out_lanes = - filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity); + const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, + std::numeric_limits::max()); // Set the maximum backward distance less than the distance from the vehicle's base_link to the // lane's rearmost point to prevent lane departure. @@ -890,9 +943,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( back_path_from_start_pose.points, start_pose.position, -back_distance); - if (!backed_pose) { + if (!backed_pose) continue; + + if (utils::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes, + parameters_->collision_check_margin_from_front_object)) continue; - } const double backed_pose_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length; @@ -909,7 +965,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( if (utils::checkCollisionBetweenFootprintAndObjects( local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, - parameters_->collision_check_margin)) { + parameters_->collision_check_margins.back())) { break; // poses behind this has a collision, so break. } @@ -919,16 +975,25 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( } PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, + const double velocity_threshold, const double object_check_forward_distance, + const double object_check_backward_distance) const { const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); - // filter for objects located in pull_out_lanes and moving at a speed below the threshold - const auto [stop_objects_in_pull_out_lanes, others] = + // filter for objects located in pull out lanes and moving at a speed below the threshold + auto [stop_objects_in_pull_out_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + const auto path = planner_data_->route_handler->getCenterLinePath( + pull_out_lanes, object_check_backward_distance, object_check_forward_distance); + + utils::path_safety_checker::filterObjectsByPosition( + stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance, + object_check_backward_distance); + return stop_objects_in_pull_out_lanes; } @@ -1142,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 @@ -1182,7 +1246,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() { BehaviorModuleOutput output; const PathWithLaneId stop_path = generateStopPath(); - output.path = std::make_shared(stop_path); + output.path = stop_path; setDrivableAreaInfo(output); @@ -1201,7 +1265,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() } path_candidate_ = std::make_shared(stop_path); - path_reference_ = getPreviousModuleOutput().reference_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); return output; } @@ -1258,7 +1322,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, generateDrivableLanes(*output.path), + output.path, generateDrivableLanes(output.path), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; @@ -1271,8 +1335,46 @@ 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; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; @@ -1283,6 +1385,7 @@ void StartPlannerModule::setDebugData() const using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; + using visualization_msgs::msg::Marker; const auto life_time = rclcpp::Duration::from_seconds(1.5); auto add = [&](MarkerArray added) { @@ -1296,27 +1399,125 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3)); 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( + 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 = 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); + if (collision_check_end_pose) { + add(createPoseMarkerArray( + *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0, + Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1), + tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + const auto footprint = transformVector( + local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose)); + const double ego_z = planner_data_->self_odometry->pose.pose.position.z; + for (size_t i = 0; i < footprint.size(); i++) { + const auto & current_point = footprint.at(i); + const auto & next_point = footprint.at((i + 1) % footprint.size()); + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + } + marker.lifetime = life_time; + debug_marker_.markers.push_back(marker); + } + } + // start pose candidates + { + MarkerArray start_pose_footprint_marker_array{}; + MarkerArray start_pose_text_marker_array{}; + const auto purple = createMarkerColor(1.0, 0.0, 1.0, 0.99); + Marker footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), purple); + Marker text_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates_idx", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); + footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + for (size_t i = 0; i < debug_data_.start_pose_candidates.size(); ++i) { + footprint_marker.id = i; + text_marker.id = i; + footprint_marker.points.clear(); + text_marker.text = "idx[" + std::to_string(i) + "]"; + 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); + } + + add(start_pose_footprint_marker_array); + 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 @@ -1389,7 +1590,7 @@ void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const status_.prev_is_safe_dynamic_objects ? "true" : "false"); logFunc(" Driving Forward: %s", status_.driving_forward ? "true" : "false"); logFunc(" Backward Driving Complete: %s", status_.backward_driving_complete ? "true" : "false"); - logFunc(" Has Stop Point: %s", status_.has_stop_point ? "true" : "false"); + logFunc(" Has Stop Pose: %s", status_.stop_pose ? "true" : "false"); logFunc("[Module State]"); logFunc(" isActivated: %s", isActivated() ? "true" : "false"); diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index 9a9466936c422..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" @@ -92,7 +91,6 @@ lanelet::ConstLanelets getPullOutLanes( const auto start_pose = planner_data->route_handler->getOriginalStartPose(); lanelet::ConstLanelet current_shoulder_lane; - lanelet::ConstLanelets shoulder_lanes; if (route_handler->getPullOutStartLane( route_handler->getShoulderLanelets(), start_pose, vehicle_width, ¤t_shoulder_lane)) { // pull out from shoulder lane 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/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index 9485f165849f2..1148379c26f42 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -21,7 +21,6 @@ behavior_velocity_planner_common geometry_msgs lanelet2_extension - libboost-dev motion_utils pluginlib rclcpp 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 429f6b5a76550..efbff7e2af51d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -27,12 +27,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -103,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 @@ -131,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; @@ -154,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) { @@ -203,7 +206,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return true; } -boost::optional BlindSpotModule::getFirstPointConflictingLanelets( +std::optional BlindSpotModule::getFirstPointConflictingLanelets( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & lanelets) const { @@ -229,73 +232,45 @@ boost::optional BlindSpotModule::getFirstPointConflictingLanelets( if (is_conflict) { return first_idx_conflicting_lanelets; } else { - return boost::none; + return std::nullopt; } } -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) { - boost::optional first_idx_conflicting_lane_opt = + 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; + return std::nullopt; } - stop_idx_ip = std::max( - first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); - } else { - boost::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(); - } - - 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( @@ -383,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; } @@ -502,7 +502,38 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( return new_lanelet; } -boost::optional BlindSpotModule::generateBlindSpotPolygons( +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, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, @@ -510,19 +541,20 @@ boost::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; } @@ -535,6 +567,7 @@ boost::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 = @@ -542,10 +575,41 @@ boost::optional BlindSpotModule::generateBlindSpotPolygons( ? (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; + } + 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(); + } + 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 = @@ -575,9 +639,27 @@ boost::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 boost::none; + return std::nullopt; } } @@ -628,13 +710,13 @@ bool BlindSpotModule::isTargetObjectType( return false; } -boost::optional BlindSpotModule::getStartPointFromLaneLet( +std::optional BlindSpotModule::getStartPointFromLaneLet( const lanelet::Id lane_id) const { lanelet::ConstLanelet lanelet = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); if (lanelet.centerline().empty()) { - return boost::none; + return std::nullopt; } const auto p = lanelet.centerline().front(); geometry_msgs::msg::Point start_point; @@ -665,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 bb78ed5771d2b..6f8450568939c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -25,13 +25,12 @@ #include #include -#include - #include #include #include #include +#include #include namespace behavior_velocity_planner @@ -40,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 @@ -52,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; }; @@ -69,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( @@ -120,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. @@ -128,7 +134,7 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return Blind spot polygons */ - boost::optional generateBlindSpotPolygons( + std::optional generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, @@ -171,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 @@ -192,7 +198,7 @@ class BlindSpotModule : public SceneModuleInterface * @param lanelets target lanelets * @return path point index */ - boost::optional getFirstPointConflictingLanelets( + std::optional getFirstPointConflictingLanelets( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & lanelets) const; @@ -201,8 +207,7 @@ class BlindSpotModule : public SceneModuleInterface * @param lane_id lane id of objective lanelet * @return end point of lanelet */ - boost::optional getStartPointFromLaneLet( - const lanelet::Id lane_id) const; + std::optional getStartPointFromLaneLet(const lanelet::Id lane_id) const; /** * @brief get straight lanelets in intersection 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 762639950b2c5..29079b99b6718 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -9,60 +9,61 @@ # param for stop position stop_position: - stop_position_threshold: 1.0 # [m] threshold to check whether the vehicle stops in front of crosswalk for yielding + stop_position_threshold: 1.0 # [m] If the ego vehicle has stopped near the stop line than this value, this module assumes itself to have achieved yielding. # For the Lanelet2 map with no explicit stop lines stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk # 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 - # param for ego's slow down velocity + # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) - # param for stuck vehicles + # params to prevent stopping on crosswalks due to another vehicle ahead stuck_vehicle: enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection - stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck - max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked - stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk - min_acc: -1.0 # min acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. + max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] - # param for pass judge logic + # 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) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering - no_stop_decision: + no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. - min_acc: -1.0 # min acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + min_acc: -1.0 # min acceleration [m/ss] + 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 crosswalk where there is a traffic signal - disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal - # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + 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] the amount of time which ego should be stopping to query whether it yields or not + 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 bicycle: true # [-] whether to look and stop by BICYCLE 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/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 3f4af4a5a1138..77de7d241c05c 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -50,6 +50,8 @@ enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; struct CollisionPoint { geometry_msgs::msg::Point collision_point{}; + std::optional crosswalk_passage_direction{ + std::nullopt}; // denote obj is passing the crosswalk along the vehicle lane double time_to_collision{}; double time_to_vehicle{}; }; diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 3830aa9edddff..9eef11e72239e 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -10,6 +10,8 @@ Shumpei Wakabayashi Takayuki Murooka Kyoichi Sugahara + Mamoru Sobue + Yuki Takagi Apache License 2.0 diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index c743d440bee85..094369b0a7a3c 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()); @@ -69,8 +68,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); - cp.stuck_vehicle_attention_range = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.required_clearance = + getOrDeclareParameter(node, ns + ".stuck_vehicle.required_clearance"); cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); cp.max_jerk_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); @@ -116,6 +115,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for target area & object cp.crosswalk_attention_range = getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.vehicle_object_cross_angle_threshold = getOrDeclareParameter( + node, ns + ".object_filtering.vehicle_object_cross_angle_threshold"); cp.look_unknown = getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); cp.look_bicycle = @@ -152,10 +153,10 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) path.header.stamp); }; - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { + for (const auto & crosswalk : crosswalk_reg_elem_map) { // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. launch(crosswalk.second.id(), crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); } @@ -178,10 +179,10 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) crosswalk_id_set = getCrosswalkIdSetOnPath( planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { + for (const auto & crosswalk : crosswalk_reg_elem_map) { crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 239eddca1fed9..cee6975df3155 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -316,6 +317,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const double ego_vel = planner_data_->current_velocity->twist.linear.x; const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -349,25 +352,38 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop + auto isVehicleType = [](const uint8_t label) { + return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE; + }; std::optional> nearest_stop_info; std::vector stop_factor_points; for (const auto & object : object_info_manager_.getObject()) { - const auto & collision_point = object.collision_point; - if (collision_point) { + const auto & collision_point_opt = object.collision_point; + if (collision_point_opt) { + const auto & collision_point = collision_point_opt.value(); const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } + if ( + isVehicleType(object.classification) && ego_crosswalk_passage_direction && + collision_point.crosswalk_passage_direction) { + const double direction_diff = tier4_autoware_utils::normalizeRadian( + collision_point.crosswalk_passage_direction.value() - + ego_crosswalk_passage_direction.value()); + if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) { + continue; + } + } stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength( - sparse_resample_path.points, ego_pos, collision_point->collision_point) - + calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) - planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { nearest_stop_info = - std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); + std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front); } } } @@ -377,7 +393,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return {}; } - // Check if the ego should stop beyond the stop line. + // Check if the ego should stop at the stop line or the other points. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; @@ -388,9 +404,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return createStopFactor(*default_stop_pose, stop_factor_points); } } else { - // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object); + ego_path.points, nearest_stop_info->first, + -base_link2front - planner_param_.stop_distance_from_object); if (stop_pose) { return createStopFactor(*stop_pose, stop_factor_points); } @@ -580,6 +596,70 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } +std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( + const PathWithLaneId & path) const +{ + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.points.size() - 1; ++i) { + const auto & start = path.points.at(i).point.pose.position; + const auto & end = path.points.at(i + 1).point.pose.position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + +std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const +{ + using tier4_autoware_utils::Segment2d; + + auto findIntersectPoint = [&](const lanelet::ConstLineString3d line) + -> std::optional> { + const auto line_start = + tier4_autoware_utils::createPoint(line.front().x(), line.front().y(), line.front().z()); + const auto line_end = + tier4_autoware_utils::createPoint(line.back().x(), line.back().y(), line.back().z()); + for (unsigned i = 0; i < path.path.size() - 1; ++i) { + const auto & start = path.path.at(i).position; + const auto & end = path.path.at(i + 1).position; + if (const auto intersect = tier4_autoware_utils::intersect(line_start, line_end, start, end); + intersect.has_value()) { + return std::make_optional(std::make_pair(i, intersect.value())); + } + } + return std::nullopt; + }; + const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound()); + const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound()); + + if (!intersect_pt1 || !intersect_pt2) { + return std::nullopt; + } + const auto idx1 = intersect_pt1.value().first, idx2 = intersect_pt2.value().first; + const auto & front = idx1 > idx2 ? intersect_pt2.value().second : intersect_pt1.value().second; + const auto & back = idx1 > idx2 ? intersect_pt1.value().second : intersect_pt2.value().second; + return std::atan2(back.y - front.y, back.x - front.x); +} + std::optional CrosswalkModule::getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) @@ -625,6 +705,8 @@ std::optional CrosswalkModule::getCollisionPoint( // Calculate multi-step object polygon, and reset start_idx const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto object_crosswalk_passage_direction = + findObjectPassageDirectionAlongVehicleLane(obj_path); const auto obj_multi_step_polygon = createMultiStepPolygon( obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); is_start_idx_initialized = false; @@ -660,7 +742,8 @@ std::optional CrosswalkModule::getCollisionPoint( if (dist_ego2cp < minimum_stop_dist) { minimum_stop_dist = dist_ego2cp; nearest_collision_point = createCollisionPoint( - intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel, + object_crosswalk_passage_direction); debug_data_.obj_polygons.push_back( toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); } @@ -679,19 +762,23 @@ std::optional CrosswalkModule::getCollisionPoint( CollisionPoint CrosswalkModule::createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const + const geometry_msgs::msg::Vector3 & obj_vel, + 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); 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; @@ -726,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; @@ -789,7 +878,7 @@ Polygon2d CrosswalkModule::getAttentionArea( std::optional CrosswalkModule::checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, - const std::optional & stop_pose) const + const std::optional & stop_pose) { const auto & p = planner_param_; @@ -817,44 +906,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( continue; } - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pose.position); if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto ego_vel = planner_data_->current_velocity->twist.linear.x; - const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - - const double near_attention_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; - - const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); - - if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { - // Plan STOP considering min_acc, max_jerk and min_jerk. - const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, - p.min_jerk_for_stuck_vehicle); - if (!min_feasible_dist_ego2stop) { - continue; + // check if STOP is required + const double crosswalk_front_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.front(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double crosswalk_back_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.back(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double required_space_length = + planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance; + + if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) { + // If there exists at least one vehicle ahead, plan STOP considering min_acc, max_jerk and + // min_jerk. Note that nearest search is not required because the stop pose independents to + // the vehicles. + const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle, + p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle); + if (!braking_distance) { + return {}; } + const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); - const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop); const double dist_to_ego = calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); - const auto feasible_stop_pose = calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); if (!feasible_stop_pose) { - continue; + return {}; } - return createStopFactor(*feasible_stop_pose, {obj_pos}); + setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED); + return createStopFactor(*feasible_stop_pose, {obj_pose.position}); } } @@ -931,13 +1024,32 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon()); + has_traffic_light, collision_point, object.classification.front().label, planner_param_, + crosswalk_.polygon2d().basicPolygon()); + const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); if (collision_point) { - const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); debug_data_.collision_points.push_back( std::make_tuple(obj_uuid, *collision_point, collision_state)); } + + const auto getLabelColor = [](const auto collision_state) { + if (collision_state == CollisionState::YIELD) { + return ColorName::RED; + } else if ( + collision_state == CollisionState::EGO_PASS_FIRST || + collision_state == CollisionState::EGO_PASS_LATER) { + return ColorName::GREEN; + } else if (collision_state == CollisionState::IGNORE) { + return ColorName::GRAY; + } else { + return ColorName::AMBER; + } + }; + + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, + getLabelColor(collision_state)); } object_info_manager_.finalize(); } @@ -948,19 +1060,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - const auto traffic_signal_stamped = + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); - if (!traffic_signal_stamped) { + if (!traffic_signal_stamped_opt) { continue; } + const auto traffic_signal_stamped = traffic_signal_stamped_opt.value(); if ( planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + (clock_->now() - traffic_signal_stamped.stamp).seconds()) { continue; } - const auto & lights = traffic_signal_stamped->signal.elements; + const auto & lights = traffic_signal_stamped.signal.elements; if (lights.empty()) { continue; } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index ef6d01fc43c23..ed1e342df9f7a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -124,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface bool enable_stuck_check_in_intersection{false}; double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; - double stuck_vehicle_attention_range; + double required_clearance; double min_acc_for_stuck_vehicle; double max_jerk_for_stuck_vehicle; double min_jerk_for_stuck_vehicle; @@ -150,6 +150,7 @@ class CrosswalkModule : public SceneModuleInterface double traffic_light_state_timeout; // param for target area & object double crosswalk_attention_range; + double vehicle_object_cross_angle_threshold; bool look_unknown; bool look_bicycle; bool look_motorcycle; @@ -160,6 +161,7 @@ class CrosswalkModule : public SceneModuleInterface { CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; + uint8_t classification{ObjectClassification::UNKNOWN}; geometry_msgs::msg::Point position{}; std::optional collision_point{}; @@ -242,8 +244,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param, - const lanelet::BasicPolygon2d & crosswalk_polygon) + const std::optional & collision_point, const uint8_t classification, + const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -265,6 +267,7 @@ class CrosswalkModule : public SceneModuleInterface crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; + objects.at(uuid).classification = classification; } void finalize() { @@ -328,7 +331,12 @@ class CrosswalkModule : public SceneModuleInterface std::optional checkStopForStuckVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, - const std::optional & stop_pose) const; + const std::optional & stop_pose); + + std::optional findEgoPassageDirectionAlongPath( + const PathWithLaneId & sparse_resample_path) const; + std::optional findObjectPassageDirectionAlongVehicleLane( + const autoware_auto_perception_msgs::msg::PredictedPath & path) const; std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, @@ -366,7 +374,8 @@ class CrosswalkModule : public SceneModuleInterface CollisionPoint createCollisionPoint( const geometry_msgs::msg::Point & nearest_collision_point, const double dist_ego2cp, const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, - const geometry_msgs::msg::Vector3 & obj_vel) const; + const geometry_msgs::msg::Vector3 & obj_vel, + const std::optional object_crosswalk_passage_direction) const; float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; 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_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt new file mode 100644 index 0000000000000..20f4b3aa8f4d3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_dynamic_obstacle_stop_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md new file mode 100644 index 0000000000000..8aa3415c3f329 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md @@ -0,0 +1,85 @@ +## Dynamic Obstacle Stop + +### Role + +`dynamic_obstacle_stop` is a module that stops the ego vehicle from entering the _immediate_ path of a dynamic object. + +The _immediate_ path of an object is the area that the object would traverse during a given time horizon, assuming constant velocity and heading. + +![rviz example](docs/dynamic_obstacle_stop_rviz.png) + +### Activation Timing + +This module is activated if the launch parameter `launch_dynamic_obstacle_stop_module` is set to true in the behavior planning launch file. + +### Inner-workings / Algorithms + +The module insert a stop point where the ego path collides with the immediate path of an object. +The overall module flow can be summarized with the following 4 steps. + +1. Filter dynamic objects. +2. Calculate immediate path rectangles of the dynamic objects. +3. Find earliest collision where ego collides with an immediate path rectangle. +4. Insert stop point before the collision. + +In addition to these 4 steps, 2 mechanisms are in place to make the stop point of this module more stable: an hysteresis and a decision duration buffer. + +The `hysteresis` parameter is used when a stop point was already being inserted in the previous iteration +and it increases the range where dynamic objects are considered close enough to the ego path to be used by the module. + +The `decision_duration_buffer` parameter defines the duration when the module will keep inserted the previous stop point, even after no collisions were found. + +#### Filter dynamic objects + +![filtering example](docs/DynamicObstacleStop-Filtering.drawio.svg) + +An object is considered by the module only if it meets all of the following conditions: + +- it is a vehicle (pedestrians are ignored); +- it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter; +- it is not too close to the current position of the ego vehicle; +- it is close to the ego path. + +For the last condition, +the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter). +In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration. + +#### Calculate immediate path rectangles + +![Immediate paths example](docs/DynamicObstacleStop-ImmediatePaths.drawio.svg) + +For each considered object, a rectangle is created representing its _immediate_ path. +The rectangle has the width of the object plus the `extra_object_width` parameter +and its length is the current speed of the object multiplied by the `time_horizon`. + +#### Find earliest collision + +![Earliest collision example](docs/DynamicObstacleStop-Collision.drawio.svg) + +We build the ego path footprints as the set of ego footprint polygons projected on each path point. +We then calculate the intersections between these ego path footprints and the previously calculated immediate path rectangles. +An intersection is ignored if the object is not driving toward ego, i.e., the absolute angle between the object and the path point is larger than $\frac{3 \pi}{4}$. + +The collision point with the lowest arc length when projected on the ego path will be used to calculate the final stop point. + +#### Insert stop point + +![Stop point example](docs/DynamicObstacleStop-StopPoint.drawio.svg) + +Before inserting a stop point, we calculate the range of path arc lengths where it can be inserted. +The minimum is calculated to satisfy the acceleration and jerk constraints of the vehicle. +If a stop point was inserted in the previous iteration of the module, its arc length is used as the maximum. +Finally, +the stop point arc length is calculated to be the arc length of the previously found collision point minus the `stop_distance_buffer` and the ego vehicle longitudinal offset, clamped between the minimum and maximum values. + +### Module Parameters + +| Parameter | Type | Description | +| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------ | +| `extra_object_width` | double | [m] extra width around detected objects | +| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | +| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | +| `time_horizon` | double | [s] time horizon used for collision checks | +| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | +| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | +| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml new file mode 100644 index 0000000000000..14483093e8bb3 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + dynamic_obstacle_stop: # module to stop or before entering the immediate path of a moving object + extra_object_width: 1.0 # [m] extra width around detected objects + minimum_object_velocity: 0.5 # [m/s] objects with a velocity bellow this value are ignored + stop_distance_buffer: 0.5 # [m] extra distance to add between the stop point and the collision point + time_horizon: 5.0 # [s] time horizon used for collision checks + hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection + decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled + minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg new file mode 100644 index 0000000000000..77beb27c20db8 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Collision.drawio.svg @@ -0,0 +1,236 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + + + + + + + +
+
+
+
abs(angle) < ¾π
+
collisions are ignored
+
+
+
+ + abs(angle) < ¾π... + + + + + + + +
+
+
+ earliest collision point +
+
+
+ + earliest collision point + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg new file mode 100644 index 0000000000000..08638931b599f --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-Filtering.drawio.svg @@ -0,0 +1,156 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + +
+
+
+ NPC 2 +
+
+
+ + NPC 2 + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + +
+
+
+
NPC 4
+
+
+
+ + NPC 4 + + + + + + + + +
+
+
+ NPC 2 is too far from the path and is ignored +
+
+
+ + NPC 2 is too far from the path and is ig... + + + + + + + + + +
+
+
+ NPC 4 is too close to ego and is ignored +
+
+
+ + NPC 4 is too close to ego and is ignored + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg new file mode 100644 index 0000000000000..3f07c95c7399d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-ImmediatePaths.drawio.svg @@ -0,0 +1,103 @@ + + + + + + + + + + +
+
+
+ NPC 1 +
+
+
+ + NPC 1 + + + + + + + + +
+
+
+ NPC 3 +
+
+
+ + NPC 3 + + + + + + + + + + + + +
+
+
+
object width +
+
extra_object_width
+
+
+
+ + object width +... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg new file mode 100644 index 0000000000000..0b0953cbea265 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/DynamicObstacleStop-StopPoint.drawio.svg @@ -0,0 +1,98 @@ + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + +
+
+
+ ego longitudinal offset + extra_distance_buffer +
+
+
+ + ego longitudinal offset + extra_distance_buffer + + + + + + + +
+
+
+ stop point +
+
+
+ + stop point + + + + + + +
+
+
+ collision point +
+
+
+ + collision... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png new file mode 100644 index 0000000000000..169cb40875d2d Binary files /dev/null and b/planning/behavior_velocity_dynamic_obstacle_stop_module/docs/dynamic_obstacle_stop_rviz.png differ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml new file mode 100644 index 0000000000000..7e9599c49bc2d --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -0,0 +1,36 @@ + + + + behavior_velocity_dynamic_obstacle_stop_module + 0.1.0 + dynamic_obstacle_stop module to stop ego when in the immediate path of a dynamic object + + Maxime Clement + Mamoru Sobue + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml new file mode 100644 index 0000000000000..2f5662c7998ac --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp new file mode 100644 index 0000000000000..46c58d6be8805 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -0,0 +1,71 @@ +// 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 "collision.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data) +{ + auto earliest_idx = ego_data.path_footprints.size(); + auto earliest_arc_length = motion_utils::calcArcLength(ego_data.path.points); + std::optional earliest_collision_point; + debug_data.collisions.clear(); + std::vector rough_collisions; + for (auto obstacle_idx = 0UL; obstacle_idx < objects.size(); ++obstacle_idx) { + rough_collisions.clear(); + const auto & obstacle_pose = objects[obstacle_idx].kinematics.initial_pose_with_covariance.pose; + const auto & obstacle_footprint = obstacle_forward_footprints[obstacle_idx]; + ego_data.rtree.query( + boost::geometry::index::intersects(obstacle_footprint), std::back_inserter(rough_collisions)); + for (const auto & rough_collision : rough_collisions) { + const auto path_idx = rough_collision.second; + const auto & ego_footprint = ego_data.path_footprints[path_idx]; + const auto & ego_pose = ego_data.path.points[ego_data.first_path_idx + path_idx].point.pose; + const auto angle_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(ego_pose.orientation) - tf2::getYaw(obstacle_pose.orientation)); + if (path_idx <= earliest_idx && std::abs(angle_diff) > (M_PI_2 + M_PI_4)) { + tier4_autoware_utils::MultiPoint2d collision_points; + boost::geometry::intersection( + obstacle_footprint.outer(), ego_footprint.outer(), collision_points); + earliest_idx = path_idx; + for (const auto & coll_p : collision_points) { + auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y()); + const auto arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, ego_data.first_path_idx, p); + if (arc_length < earliest_arc_length) { + earliest_arc_length = arc_length; + debug_data.collisions = {obstacle_footprint, ego_footprint}; + earliest_collision_point = p; + } + } + } + } + } + return earliest_collision_point; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp new file mode 100644 index 0000000000000..ccc3fa94df603 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.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 COLLISION_HPP_ +#define COLLISION_HPP_ + +#include "types.hpp" + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief find the earliest collision along the ego path and an obstacle footprint +/// @param [in] ego_data ego data including its path and footprint +/// @param [in] objects obstacles +/// @param [in] obstacle_forward_footprints obstacle footprints +/// @param [in] debug_data debug data +/// @return the point of earliest collision along the ego path +std::optional find_earliest_collision( + const EgoData & ego_data, + const std::vector & objects, + const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints, DebugData & debug_data); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp new file mode 100644 index 0000000000000..cc5dafa218654 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -0,0 +1,87 @@ +// 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 "debug.hpp" + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ + +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETE; + marker.ns = ns; + for (marker.id = static_cast(from); marker.id < static_cast(to); ++marker.id) + markers.push_back(marker); + return markers; +} + +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = "dynamic_obstacles"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0); + marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 1.0); + marker.text = "dynamic obstacle"; + for (const auto & obstacle : obstacles) { + marker.pose = obstacle.kinematics.initial_pose_with_covariance.pose; + markers.push_back(marker); + ++marker.id; + } + return markers; +} + +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = ns; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.8); + for (const auto & footprint : footprints) { + marker.points.clear(); + for (const auto & p : footprint.outer()) { + marker.points.emplace_back(); + marker.points.back().x = p.x(); + marker.points.back().y = p.y(); + marker.points.back().z = z; + } + markers.push_back(marker); + ++marker.id; + } + return markers; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp new file mode 100644 index 0000000000000..02f550d314d39 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -0,0 +1,40 @@ +// 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 DEBUG_HPP_ +#define DEBUG_HPP_ + +#include "types.hpp" + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +{ +std::vector make_delete_markers( + const size_t from, const size_t to, const std::string & ns); +std::vector make_dynamic_obstacle_markers( + const std::vector & obstacles); +std::vector make_polygon_markers( + const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); +std::vector make_collision_markers( + const tier4_autoware_utils::MultiPoint2d & collisions, const double z); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug + +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp new file mode 100644 index 0000000000000..6a0c61963ac81 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -0,0 +1,84 @@ +// 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 "footprint.hpp" + +#include + +#include + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis) +{ + tier4_autoware_utils::MultiPolygon2d forward_footprints; + for (const auto & obstacle : obstacles) + forward_footprints.push_back(project_to_pose( + make_forward_footprint(obstacle, params, hysteresis), + obstacle.kinematics.initial_pose_with_covariance.pose)); + return forward_footprints; +} + +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis) +{ + const auto & shape = obstacle.shape.dimensions; + const auto longitudinal_offset = + shape.x / 2.0 + + obstacle.kinematics.initial_twist_with_covariance.twist.linear.x * params.time_horizon; + const auto lateral_offset = (shape.y + params.extra_object_width) / 2.0 + hysteresis; + return tier4_autoware_utils::Polygon2d{ + {{longitudinal_offset, -lateral_offset}, + {longitudinal_offset, lateral_offset}, + {shape.x / 2.0, lateral_offset}, + {shape.x / 2.0, -lateral_offset}, + {longitudinal_offset, -lateral_offset}}, + {}}; +} + +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) +{ + const auto angle = tf2::getYaw(pose.orientation); + const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); + tier4_autoware_utils::Polygon2d footprint; + for (const auto & p : rotated_footprint.outer()) + footprint.outer().emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); + return footprint; +} + +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) +{ + for (const auto & p : ego_data.path.points) + ego_data.path_footprints.push_back(tier4_autoware_utils::toFootprint( + p.point.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); + for (auto i = 0UL; i < ego_data.path_footprints.size(); ++i) { + const auto box = + boost::geometry::return_envelope(ego_data.path_footprints[i]); + ego_data.rtree.insert(std::make_pair(box, i)); + } +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp new file mode 100644 index 0000000000000..c28e89ac6c9f6 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -0,0 +1,55 @@ +// 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 FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +/// @brief create the footprint of the given obstacles and their projection over a fixed time +/// horizon +/// @param [in] obstacles obstacles +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprints +/// @return forward footprint of the obstacle +tier4_autoware_utils::MultiPolygon2d make_forward_footprints( + const std::vector & obstacles, + const PlannerParam & params, const double hysteresis); +/// @brief create the footprint of the given obstacle and its projection over a fixed time horizon +/// @param [in] obstacle obstacle +/// @param [in] params parameters used to create the footprint +/// @param [in] hysteresis [m] extra lateral distance to add to the footprint +/// @return forward footprint of the obstacle +tier4_autoware_utils::Polygon2d make_forward_footprint( + const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const double hysteresis); +/// @brief project a footprint to the given pose +/// @param [in] base_footprint footprint to project +/// @param [in] pose projection pose +/// @return footprint projected to the given pose +tier4_autoware_utils::Polygon2d project_to_pose( + const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); +/// @brief create the rtree indexing the ego footprint along the path +/// @param [inout] ego_data ego data with its path and the rtree to populate +/// @param [in] params parameters +void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp new file mode 100644 index 0000000000000..99981007b20c2 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -0,0 +1,73 @@ +// 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 "manager.hpp" + +#include "scene_dynamic_obstacle_stop.hpp" + +#include + +#include + +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::getOrDeclareParameter; + +DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) +{ + const std::string ns(getModuleName()); + auto & pp = planner_param_; + + pp.extra_object_width = getOrDeclareParameter(node, ns + ".extra_object_width"); + pp.minimum_object_velocity = getOrDeclareParameter(node, ns + ".minimum_object_velocity"); + pp.stop_distance_buffer = getOrDeclareParameter(node, ns + ".stop_distance_buffer"); + pp.time_horizon = getOrDeclareParameter(node, ns + ".time_horizon"); + pp.hysteresis = getOrDeclareParameter(node, ns + ".hysteresis"); + pp.decision_duration_buffer = + getOrDeclareParameter(node, ns + ".decision_duration_buffer"); + pp.minimum_object_distance_from_ego_path = + getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); + + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.ego_lateral_offset = + std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); + pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; +} + +void DynamicObstacleStopModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) return; + // general + if (!isModuleRegistered(module_id_)) { + registerModule(std::make_shared( + module_id_, planner_param_, logger_.get_child("dynamic_obstacle_stop_module"), clock_)); + } +} + +std::function &)> +DynamicObstacleStopModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + return false; + }; +} +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::DynamicObstacleStopModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp new file mode 100644 index 0000000000000..e461cc9c16445 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -0,0 +1,58 @@ +// 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene_dynamic_obstacle_stop.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface +{ +public: + explicit DynamicObstacleStopModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "dynamic_obstacle_stop"; } + +private: + using PlannerParam = dynamic_obstacle_stop::PlannerParam; + + PlannerParam planner_param_; + int64_t module_id_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class DynamicObstacleStopModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp new file mode 100644 index 0000000000000..0411ab2d01cfe --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -0,0 +1,71 @@ +// 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 "object_filtering.hpp" + +#include "types.hpp" + +#include + +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis) +{ + std::vector filtered_objects; + const auto is_vehicle = [](const auto & o) { + return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + }) != o.classification.end(); + }; + const auto is_in_range = [&](const auto & o) { + const auto distance = std::abs(motion_utils::calcLateralOffset( + ego_data.path.points, o.kinematics.initial_pose_with_covariance.pose.position)); + return distance <= params.minimum_object_distance_from_ego_path + params.ego_lateral_offset + + o.shape.dimensions.y / 2.0 + hysteresis; + }; + const auto is_not_too_close = [&](const auto & o) { + const auto obj_arc_length = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, + o.kinematics.initial_pose_with_covariance.pose.position); + return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx + + params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; + }; + for (const auto & object : objects.objects) + if ( + is_vehicle(object) && + object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity && + is_in_range(object) && is_not_too_close(object)) + filtered_objects.push_back(object); + return filtered_objects; +} +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp new file mode 100644 index 0000000000000..22857f6db1bda --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.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 OBJECT_FILTERING_HPP_ +#define OBJECT_FILTERING_HPP_ + +#include "types.hpp" + +#include + +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +/// @brief filter the given predicted objects +/// @param objects predicted objects +/// @param ego_data ego data, including its path and pose +/// @param params parameters +/// @param hysteresis [m] extra distance threshold used for filtering +/// @return filtered predicted objects +std::vector filter_predicted_objects( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const PlannerParam & params, const double hysteresis); + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp new file mode 100644 index 0000000000000..98b7984bab1dc --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -0,0 +1,185 @@ +// Copyright 2023 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_dynamic_obstacle_stop.hpp" + +#include "collision.hpp" +#include "debug.hpp" +#include "footprint.hpp" +#include "object_filtering.hpp" +#include "types.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +DynamicObstacleStopModule::DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) +{ + prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type()); + velocity_factor_.init(PlanningBehavior::UNKNOWN); +} + +bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_.reset_data(); + *stop_reason = planning_utils::initializeStopReason(StopReason::OBSTACLE_STOP); + if (!path || path->points.size() < 2) return true; + tier4_autoware_utils::StopWatch stopwatch; + stopwatch.tic(); + + stopwatch.tic("preprocessing"); + EgoData ego_data; + ego_data.pose = planner_data_->current_odometry->pose; + ego_data.path.points = path->points; + motion_utils::removeOverlapPoints(ego_data.path.points); + ego_data.first_path_idx = + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( + ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); + + make_ego_footprint_rtree(ego_data, params_); + const auto has_decided_to_stop = + (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; + if (!has_decided_to_stop) current_stop_pose_.reset(); + double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0; + const auto dynamic_obstacles = + filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_, hysteresis); + + const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); + + stopwatch.tic("footprints"); + const auto obstacle_forward_footprints = + make_forward_footprints(dynamic_obstacles, params_, hysteresis); + const auto footprints_duration_us = stopwatch.toc("footprints"); + const auto min_stop_distance = + motion_utils::calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, + planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, + planner_data_->max_stop_jerk_threshold) + .value_or(0.0); + stopwatch.tic("collisions"); + const auto collision = + find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_); + const auto collisions_duration_us = stopwatch.toc("collisions"); + if (collision) { + const auto arc_length_diff = + motion_utils::calcSignedArcLength(ego_data.path.points, *collision, ego_data.pose.position); + const auto can_stop_before_limit = arc_length_diff < min_stop_distance - + params_.ego_longitudinal_offset - + params_.stop_distance_buffer; + const auto stop_pose = can_stop_before_limit + ? motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, *collision, + -params_.stop_distance_buffer - params_.ego_longitudinal_offset) + : motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, ego_data.pose.position, min_stop_distance); + if (stop_pose) { + const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength( + path->points, stop_pose->position, + current_stop_pose_->position) > 0.0; + if (use_new_stop_pose) current_stop_pose_ = *stop_pose; + prev_stop_decision_time_ = clock_->now(); + } + } + + if (current_stop_pose_) motion_utils::insertStopPoint(*current_stop_pose_, 0.0, path->points); + + const auto total_time_us = stopwatch.toc(); + RCLCPP_DEBUG( + logger_, + "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = " + "%2.2fus\n\tcollisions = %2.2fus\n", + total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us); + debug_data_.ego_footprints = ego_data.path_footprints; + debug_data_.obstacle_footprints = obstacle_forward_footprints; + return true; +} + +MarkerArray DynamicObstacleStopModule::createDebugMarkerArray() +{ + constexpr auto z = 0.0; + MarkerArray debug_marker_array; + // dynamic obstacles footprints + const auto obstacle_footprint_markers = + debug::make_polygon_markers(debug_data_.obstacle_footprints, "dynamic_obstacles_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), obstacle_footprint_markers.begin(), + obstacle_footprint_markers.end()); + const auto delete_footprint_markers = debug::make_delete_markers( + obstacle_footprint_markers.size(), debug_data_.prev_dynamic_obstacles_nb, + "dynamic_obstacles_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_footprint_markers.begin(), + delete_footprint_markers.end()); + // ego path footprints + const auto ego_footprint_markers = + debug::make_polygon_markers(debug_data_.ego_footprints, "ego_footprints", z); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), ego_footprint_markers.begin(), ego_footprint_markers.end()); + const auto delete_ego_footprint_markers = debug::make_delete_markers( + ego_footprint_markers.size(), debug_data_.prev_ego_footprints_nb, "ego_footprints"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_ego_footprint_markers.begin(), + delete_ego_footprint_markers.end()); + // collisions + auto collision_markers = debug::make_polygon_markers(debug_data_.collisions, "collisions", z); + for (auto & m : collision_markers) m.color.r = 1.0; + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), collision_markers.begin(), collision_markers.end()); + const auto delete_collision_markers = debug::make_delete_markers( + collision_markers.size(), debug_data_.prev_collisions_nb, "collisions"); + debug_marker_array.markers.insert( + debug_marker_array.markers.end(), delete_collision_markers.begin(), + delete_collision_markers.end()); + + debug_data_.prev_dynamic_obstacles_nb = obstacle_footprint_markers.size(); + debug_data_.prev_collisions_nb = collision_markers.size(); + debug_data_.prev_ego_footprints_nb = ego_footprint_markers.size(); + return debug_marker_array; +} + +motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls{}; + if (current_stop_pose_) { + motion_utils::VirtualWall virtual_wall; + virtual_wall.text = "dynamic_obstacle_stop"; + virtual_wall.longitudinal_offset = params_.ego_longitudinal_offset; + virtual_wall.style = motion_utils::VirtualWallType::stop; + virtual_wall.pose = *current_stop_pose_; + virtual_walls.push_back(virtual_wall); + } + return virtual_walls; +} + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp new file mode 100644 index 0000000000000..c7bca149890a0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -0,0 +1,62 @@ +// 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 SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ +#define SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ + +#include "types.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +class DynamicObstacleStopModule : public SceneModuleInterface +{ +public: + DynamicObstacleStopModule( + const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock); + + /// @brief insert stop or slow down points to prevent dangerously entering another lane + /// @param [inout] path the path to update + /// @param [inout] stop_reason reason for stopping + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + motion_utils::VirtualWalls createVirtualWalls() override; + +private: + PlannerParam params_; + rclcpp::Time prev_stop_decision_time_; + std::optional current_stop_pose_; + +protected: + int64_t module_id_{}; + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp new file mode 100644 index 0000000000000..74fd5ca818fb0 --- /dev/null +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -0,0 +1,80 @@ +// 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 TYPES_HPP_ +#define TYPES_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner::dynamic_obstacle_stop +{ +typedef std::pair BoxIndexPair; +typedef boost::geometry::index::rtree> Rtree; + +/// @brief parameters for the "out of lane" module +struct PlannerParam +{ + double extra_object_width; + double minimum_object_velocity; + double stop_distance_buffer; + double time_horizon; + double hysteresis; + double decision_duration_buffer; + double ego_longitudinal_offset; + double ego_lateral_offset; + double minimum_object_distance_from_ego_path; +}; + +struct EgoData +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + size_t first_path_idx{}; + double longitudinal_offset_to_first_path_idx; // [m] + geometry_msgs::msg::Pose pose; + tier4_autoware_utils::MultiPolygon2d path_footprints; + Rtree rtree; +}; + +/// @brief debug data +struct DebugData +{ + tier4_autoware_utils::MultiPolygon2d obstacle_footprints{}; + size_t prev_dynamic_obstacles_nb{}; + tier4_autoware_utils::MultiPolygon2d collisions{}; + size_t prev_collisions_nb{}; + tier4_autoware_utils::MultiPolygon2d ego_footprints{}; + size_t prev_ego_footprints_nb{}; + std::optional stop_pose{}; + void reset_data() + { + obstacle_footprints.clear(); + collisions.clear(); + ego_footprints.clear(); + stop_pose.reset(); + } +}; + +} // namespace behavior_velocity_planner::dynamic_obstacle_stop + +#endif // TYPES_HPP_ 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 12455a26d2f4a..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 | @@ -184,6 +184,12 @@ If there is any object on the path inside the intersection and at the exit of th ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) +## Yield stuck vehicle detection + +If there is any stopped object on the attention lanelet between the intersection point with ego path and the position which is `yield_stuck.distance_threshold` before that position, the object is regarded as yielding to ego vehicle. In this case ego is given the right-of-way by the yielding object but this module inserts stopline to prevent entry into the intersection. This scene happens when the object is yielding against ego or the object is waiting before the crosswalk around the exit of the intersection. + +![yield_stuck_detection](./docs/yield-stuck.drawio.svg) + ## Collision detection The following process is performed for the targets objects to determine whether ego can pass the intersection safely. If it is judged that ego cannot pass the intersection with enough margin, this module inserts a stopline on the path. @@ -213,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. @@ -235,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) @@ -257,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 @@ -271,20 +277,53 @@ If the traffic light color is RED or Arrow signal is turned on, the attention la When the traffic light color/shape is RED/Arrow, occlusion detection is skipped. +![traffic-light-specific-behavior](./docs/traffic-light-specific-behavior.drawio.svg) + ## 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: + +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 which is before the boundary of unprotected area by the braking distance which is obtained by + +$$ +\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) -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. +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. -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. +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. -- 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. +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 @@ -367,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 @@ -422,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 1e6ce843de528..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: @@ -23,21 +24,19 @@ stuck_vehicle_velocity_threshold: 0.833 # enable_front_car_decel_prediction: false # assumed_front_car_decel: 1.0 - timeout_private_area: 3.0 - enable_private_area_stuck_disregard: false + disable_against_private_lane: true yield_stuck: turn_direction: left: true - right: false + right: true straight: false - distance_threshold: 1.0 + distance_threshold: 5.0 collision_detection: 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 @@ -73,7 +72,7 @@ enable: false creep_velocity: 0.8333 peeking_offset: -0.5 - occlusion_required_clearance_distance: 55 + occlusion_required_clearance_distance: 55.0 possible_object_bbox: [1.5, 2.5] ignore_parked_vehicle_speed_threshold: 0.8333 occlusion_detection_hold_time: 1.5 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/docs/traffic-light-specific-behavior.drawio.svg b/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg new file mode 100644 index 0000000000000..0ba5f8e41d74b --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg @@ -0,0 +1,1536 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + with traffic light(left-hand) +
+ +
+ right +
+
+ Just after changed to +
+ GREEN +
+
+ +
+
+
+ + with traffic light(left-hand)... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + with traffic light(left-hand) +
+
+ right +
+
+ while YELLOW +
+ +
+
+
+ + with traffic light(left-hand)... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + with traffic light(left-hand) +
+
+ right +
+
+ while RED/arrow + + +
+ +
+
+
+ + with traffic light(left-hand)... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + stop for a specific duration +
+ regardless of collision detection +
+ +
+
+
+ + stop for a specific duration... + + + + + +
+
+
+ + If the vehicles are not +
+ fully accelerated around +
+ the stopline +
+ +
+
+
+ + If the vehicles are not... + + + + + + + + + +
+
+
+ + relaxed collision checking +
+ compared to GREEN +
+ +
+
+
+ + relaxed collision checking... + + + + + +
+
+
+ + If the vehicle is expected to +
+ stop before the stopline under +
+ the deceleration, ignore it +
+
+ Otherwise check collision +
+ +
+
+
+ + If the vehicle is expected to... + + + + + + + + + + +
+
+
+ + more relaxed collision +
+ checking compared to YELLOW +
+ +
+
+
+ + more relaxed collision... + + + + + +
+
+
+ + Even if the vehicle passed the stopline +
+ but is expected to stop with more than a +
+ specific margin with ego path under the +
+ deceleration, ignore it +
+
+ Only check collision with vehicles +
+ inside intersection +
+ +
+
+
+ + Even if the vehicle passed the stopline... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg new file mode 100644 index 0000000000000..e0078540ba407 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg @@ -0,0 +1,750 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + w/ traffic light, right +
+
+ (Left-hand traffic) + + + +
+ + +
+
+
+ + w/ traffic light, right... + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention lane +
+
+
+ + attention lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + T-shape junction w/o traffic light +
+
+ Right-hand traffic +
+ +
+
+
+ + T-shape junction w/o traffic light... + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + + + + + + + +
+
+
+ + yield stuck detect area + +
+
+
+ + yield stuck detect area + + + + + + + + + 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 8d9beb34d05ee..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); @@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.first_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( @@ -196,6 +212,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.yield_stuck_detect_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, + 0.34509, 0.6588235), + &debug_marker_array); + } + if (debug_data_.ego_lane) { appendMarkerArray( createLaneletPolygonsMarkerArray( @@ -259,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_.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_.pass_judge_wall_pose.value(), "pass_judge_wall_pose", module_id_, 0.7, 0.85, - 0.9), + 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/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp new file mode 100644 index 0000000000000..c47f016fbdbda --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -0,0 +1,49 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef INTERPOLATED_PATH_INFO_HPP_ +#define INTERPOLATED_PATH_INFO_HPP_ + +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief wrapper class of interpolated path with lane id + */ +struct InterpolatedPathInfo +{ + /** the interpolated path */ + autoware_auto_planning_msgs::msg::PathWithLaneId path; + /** discretization interval of interpolation */ + double ds{0.0}; + /** the intersection lanelet id */ + lanelet::Id lane_id{0}; + /** the associative lane ids of lane_id */ + std::set associative_lane_ids{}; + /** the range of indices for the path points with associative lane id */ + std::optional> lane_id_interval{std::nullopt}; +}; + +} // namespace behavior_velocity_planner::intersection + +#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 789708abe98f8..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"); @@ -74,10 +75,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_velocity_threshold = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); - ip.stuck_vehicle.timeout_private_area = - getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); - ip.stuck_vehicle.enable_private_area_stuck_disregard = - getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); ip.yield_stuck.turn_direction.left = getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); @@ -94,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( @@ -154,6 +151,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); ip.occlusion.peeking_offset = getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); ip.occlusion.possible_object_bbox = getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = @@ -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(); @@ -201,7 +198,6 @@ void IntersectionModuleManager::launchNewModules( } const std::string location = ll.attributeOr("location", "else"); - const bool is_private_area = (location.compare("private") == 0); const auto associative_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); bool has_traffic_light = false; @@ -213,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, is_private_area, 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 0d483b501d1ee..90242dc3edd7e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,88 +16,35 @@ #include "util.hpp" -#include +#include // for toGeomPoly #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 + 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 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, const bool is_private_area, rclcpp::Node & node, + 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), @@ -105,9 +52,6 @@ IntersectionModule::IntersectionModule( 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), - is_private_area_(is_private_area), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -146,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); @@ -157,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 @@ -179,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) @@ -189,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) { @@ -209,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) { @@ -224,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) { @@ -243,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) { @@ -262,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) { @@ -281,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) { @@ -298,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) { @@ -317,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; @@ -336,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) { @@ -354,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; @@ -369,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 @@ -377,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, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -387,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]] util::DebugData * debug_data) + [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; } @@ -401,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, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -449,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, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -481,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, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -524,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, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -575,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, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -632,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, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -679,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, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -732,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, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -774,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, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -814,602 +1040,98 @@ 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, util::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) +bool IntersectionModule::isGreenSolidOn() const { - 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"; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; + if (!tl_id) { + // this lane has no traffic light + return false; } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { + // the info of this traffic light is not available + return false; } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; + 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 ""; + return false; } -bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - debug_data_ = util::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; -} - -static bool isGreenSolidOn( - lanelet::ConstLanelet lane, const std::map & tl_infos) +IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); + + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; } if (!tl_id) { // this lane has no traffic light - return false; + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { - // the info of this traffic light is not available - 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 auto & tl_info = tl_info_it->second; + 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::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; - - // 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 & current_pose = planner_data_->current_odometry->pose; - const auto lanelets_on_path = - planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); - if (!intersection_lanelets_) { - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.common.attention_area_length, - planner_param_.occlusion.occlusion_attention_area_length, - planner_param_.collision_detection.consider_wrong_direction_vehicle); - } - 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 = - util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); - const bool is_prioritized = - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); - - // 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(); - - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes - const auto & dummy_first_attention_area = - first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; - const auto & dummy_first_attention_lane_centerline = - intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value().centerline2d() - : first_conflicting_lane.centerline2d(); - const auto intersection_stoplines_opt = util::generateIntersectionStopLines( - first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline, - planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.default_stopline_margin, planner_param_.common.max_accel, - planner_param_.common.max_jerk, planner_param_.common.delay_response_time, - planner_param_.occlusion.peeking_offset, 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, stuck_stopline_idx_opt, default_stopline_idx_opt, - first_attention_stopline_idx_opt, occlusion_peeking_stopline_idx_opt, - default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; - - // see the doc for struct PathLanelets - const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, - conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); - 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 = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected && stuck_stopline_idx_opt) { - auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); - if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { - if ( - default_stopline_idx_opt && - fromEgoDist(stuck_stopline_idx) < -planner_param_.common.stopline_overshoot_margin) { - stuck_stopline_idx = default_stopline_idx_opt.value(); - } - } else { - return IntersectionModule::StuckStop{ - closest_idx, stuck_stopline_idx, occlusion_peeking_stopline_idx_opt}; - } - } - - // yield stuck vehicle detection is viable even if attention area is empty - // so this needs to be checked before attention area validation - const bool yield_stuck_detected = checkYieldStuckVehicle( - planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); - if (yield_stuck_detected && stuck_stopline_idx_opt) { - const auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); - return IntersectionModule::YieldStuckStop{closest_idx, stuck_stopline_idx}; - } - - // 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(); - - // check occlusion on detection lane - if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( - occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution, - planner_param_.occlusion.attention_lane_crop_curvature_threshold, - planner_param_.occlusion.attention_lane_curvature_calculation_ds); - } - 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 double is_amber_or_red = - (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); - auto occlusion_status = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? getOcclusionStatus( - *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area, interpolated_path_info, occlusion_attention_divisions, - target_objects, current_pose, - planner_param_.occlusion.occlusion_required_clearance_distance) - : 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, planner_data_->traffic_light_id_map); - 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}; + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; } - // 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}; + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; } - } 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}; - } -} - -bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) -{ - 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 auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); - debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - - return util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, &debug_data_); -} - -bool IntersectionModule::checkYieldStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area) -{ - if (!first_attention_area) { - return false; } - - 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; + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; } - - const auto & objects_ptr = planner_data->predicted_objects; - - const auto & ego_lane = path_lanelets.ego_or_entry2exit; - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - - return util::checkYieldStuckVehicleInIntersection( - objects_ptr, ego_poly, first_attention_area.value(), - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, - planner_param_.yield_stuck.distance_threshold, &debug_data_); + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } -util::TargetObjects IntersectionModule::generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, +TargetObjects IntersectionModule::generateTargetObjects( + const intersection::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - util::TargetObjects target_objects; + TargetObjects target_objects; target_objects.header = objects_ptr->header; const auto & attention_lanelets = intersection_lanelets.attention(); const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); @@ -1422,10 +1144,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects( // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, false); + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); if (belong_adjacent_lanelet_id) { continue; } @@ -1439,33 +1159,28 @@ util::TargetObjects IntersectionModule::generateTargetObjects( const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); if (belong_attention_lanelet_id) { const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); container.push_back(target_object); } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = std::nullopt; target_object.stopline = std::nullopt; target_objects.intersection_area_objects.push_back(target_object); } - } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, - planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( + object_direction, attention_lanelets, is_parked_vehicle); belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); @@ -1484,587 +1199,118 @@ util::TargetObjects IntersectionModule::generateTargetObjects( return target_objects; } -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) +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) { - 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 = util::calcIntersectionPassingTime( - path, planner_data_, lane_id_, associative_ids_, closest_idx, - last_intersection_stopline_candidate_idx, time_delay, - planner_param_.collision_detection.velocity_profile.default_velocity, - planner_param_.collision_detection.velocity_profile.minimum_default_velocity, - planner_param_.collision_detection.velocity_profile.use_upstream, - planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity, - &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; - util::cutPredictPathWithDuration(target_objects, clock_, 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 == util::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 == util::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 util::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 util::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 == util::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 == util::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 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; } - } - 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; + 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; } - } - 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; + } 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 first_pass_judge_line_idx; } } + return first_pass_judge_line_idx; + }(); + + const bool was_safe = std::holds_alternative(prev_decision_result_); + + 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(); } + 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(); + const bool is_over_default_stopline = + util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); 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); + ((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 collision_detected; + return intersection::Result>::make_err( + std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); } -IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, - 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 util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr) +void TargetObject::calc_dist_to_stopline() { - 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; + if (!attention_lanelet || !stopline) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stopline_val = stopline.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; + stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; + stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); } /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 4c33c0960afc3..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,21 +30,46 @@ #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 + * @brief see the document for more details of IntersectionStopLines + */ +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + std::optional dist_to_stopline{std::nullopt}; + void calc_dist_to_stopline(); +}; + +/** + * @struct + * @brief categorize TargetObjects + */ +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; class IntersectionModule : public SceneModuleInterface { @@ -59,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 @@ -74,8 +104,7 @@ class IntersectionModule : public SceneModuleInterface bool use_stuck_stopline; double stuck_vehicle_detect_dist; double stuck_vehicle_velocity_threshold; - double timeout_private_area; - bool enable_private_area_stuck_disregard; + bool disable_against_private_lane; } stuck_vehicle; struct YieldStuck @@ -89,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; @@ -160,117 +188,87 @@ class IntersectionModule : public SceneModuleInterface }; enum OcclusionType { + //! occlusion is not detected NOT_OCCLUDED, + //! occlusion is not caused by dynamic objects STATICALLY_OCCLUDED, + //! occlusion is caused by dynamic objects DYNAMICALLY_OCCLUDED, - RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + //! actual occlusion does not exist, only disapproved by RTC + RTC_OCCLUDED, }; - struct Indecisive - { - std::string error; - }; - struct StuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - std::optional occlusion_stopline_idx{std::nullopt}; - }; - struct YieldStuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - }; - struct NonOccludedCollisionStop - { - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - }; - 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}; - }; - // A state peeking to occlusion limit line in the presence of traffic light - struct PeekingTowardOcclusion - { - // NOTE: 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}; - }; - // A state detecting both collision and occlusion 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 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 Safe + struct DebugData { - // NOTE: if RTC is disapproved status, default stop lines are still needed. - 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}; }; - struct FullyPrioritized - { - bool collision_detected{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; + + using TimeDistanceArray = std::vector>; + + /** + * @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 }; - 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, const bool is_private_area, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** - * @brief plan go-stop velocity at traffic crossing with collision check between reference path - * and object predicted path + *********************************************************** + *********************************************************** + *********************************************************** + * @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; @@ -285,80 +283,439 @@ 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_; - bool is_go_out_{false}; - bool is_permanent_go_{false}; - DecisionResult prev_decision_result_{Indecisive{""}}; - OcclusionType prev_occlusion_status_; + //! RTC uuid for INTERSECTION_OCCLUSION + const UUID occlusion_uuid_; + /** @}*/ - // Parameter +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}; - // for occlusion detection - const bool enable_occlusion_detection_; + //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ std::nullopt}; - StateMachine collision_state_machine_; //! for stable collision checking - StateMachine before_creep_state_machine_; //! for two phase stop + /** @}*/ + +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}; + + //! 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_; + + //! 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_; + /** @} */ - // for pseudo-collision detection when ego just entered intersection on green light and upcoming - // vehicles are very slow std::optional initial_green_light_observed_time_{std::nullopt}; - // for stuck vehicle detection - const bool is_private_area_; - - // 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}; - // for first stop in two-phase stop bool occlusion_first_stop_required_{false}; + /** @}*/ +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @ingroup primary-functions + * @{ + */ + /** + * @brief set all RTC variable to true(safe) and -INF + */ void initializeRTCStatus(); + + /** + * @brief analyze traffic_light/occupancy/objects context and return DecisionResult + */ + intersection::DecisionResult modifyPathVelocityDetail( + PathWithLaneId * path, StopReason * stop_reason); + + /** + * @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); + + /** + * @brief act based on current RTC approval + */ + void reactRTCApproval( + const intersection::DecisionResult & decision_result, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + /** @}*/ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup prepare-data [fn] basic data construction + * @{ + */ + /** + * @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); + + /** + * @brief generate IntersectionStopLines + */ + 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 intersection::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @brief generate IntersectionLanelets + */ + intersection::IntersectionLanelets generateObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet); + + /** + * @brief generate PathLanelets + */ + std::optional 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); + + /** + * @brief generate discretized detection lane linestring. + */ + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + /** @} */ + +private: + /** + * @defgroup utility [fn] utility member function + * @{ + */ + void stoppedAtPositionForDuration( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, + const double duration, StateMachine * state_machine); + /** @} */ - bool checkStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets); +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup get-traffic-light [fn] traffic light + * @{ + */ + /** + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn() const; - bool checkYieldStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets, - const std::optional & first_attention_area); + /** + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; + /** @} */ - util::TargetObjects generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, +private: + /** + * @brief categorize target objects + */ + TargetObjects generateTargetObjects( + const intersection::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; - bool checkCollision( +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, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); + 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; + /** @} */ - OcclusionType getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, +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 util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr); + 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); + + /** + * @brief check collision + */ + bool checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const intersection::PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level); + + std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const; + + void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); + + /** + * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of + * (time of arrival, traveled distance) from current ego position + */ + TimeDistanceArray calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); + /** @} */ /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -368,7 +725,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - util::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 b373f2cbc1c8a..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 @@ -16,17 +16,17 @@ #include "util.hpp" -#include #include #include #include -#include #include #include #include #include +#include +#include #include #include #include @@ -50,6 +50,32 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( state_machine_.setState(StateMachine::State::STOP); } +static std::optional getFirstConflictingLanelet( + const lanelet::ConstLanelets & conflicting_lanelets, + 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, 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 <= 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 (const auto & conflicting_lanelet : conflicting_lanelets) { + const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); + const bool intersects = bg::intersects(polygon_2d, path_footprint); + if (intersects) { + return std::make_optional(conflicting_lanelet); + } + } + } + return std::nullopt; +} + bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = DebugData(); @@ -83,44 +109,37 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return false; } - /* get detection area */ - if (!intersection_lanelets_) { - const auto & assigned_lanelet = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - *path, lanelet_map_ptr, planner_data_->current_odometry->pose); - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, - planner_param_.consider_wrong_direction_vehicle); - } - auto & intersection_lanelets = intersection_lanelets_.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update( - false, interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); - const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); - if (!first_conflicting_area) { + if (!first_conflicting_lanelet_) { + const auto conflicting_lanelets = getAttentionLanelets(); + first_conflicting_lanelet_ = getFirstConflictingLanelet( + conflicting_lanelets, interpolated_path_info, local_footprint, baselink2front); + } + if (!first_conflicting_lanelet_) { return false; } + const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - /* set stop-line and stop-judgement-line for base_link */ - const auto stopline_idx_opt = util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, interpolated_path_info, - planner_param_.stopline_margin, false, path); - if (!stopline_idx_opt.has_value()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); + 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; } - - const size_t stopline_idx = stopline_idx_opt.value(); - if (stopline_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); + const auto stopline_idx_ip = static_cast(std::max( + 0, static_cast(first_conflicting_idx_opt.value()) - + static_cast(baselink2front / planner_param_.path_interpolation_ds))); + + const auto stopline_idx_opt = util::insertPointIndex( + interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + if (!stopline_idx_opt) { + RCLCPP_DEBUG(logger_, "failed to insert stopline, ignore planning."); return true; } + const auto stopline_idx = stopline_idx_opt.value(); - debug_data_.virtual_wall_pose = planning_utils::getAheadPose( - stopline_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); debug_data_.stop_point_pose = path->points.at(stopline_idx).point.pose; /* set stop speed */ @@ -154,45 +173,33 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return true; } -autoware_auto_planning_msgs::msg::PathWithLaneId -MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length) +lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const { - if (path.points.size() < 2) { - return path; - } - - autoware_auto_planning_msgs::msg::PathWithLaneId private_path = path; - private_path.points.clear(); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - double sum_dist = 0.0; - bool prev_has_target_lane_id = false; - for (int i = static_cast(path.points.size()) - 2; i >= 0; i--) { - bool has_target_lane_id = false; - for (const auto path_id : path.points.at(i).lane_ids) { - if (path_id == lane_id_) { - has_target_lane_id = true; + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + lanelet::ConstLanelets sibling_lanelets; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + sibling_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(sibling_lanelets, following_lanelet)) { + continue; } + sibling_lanelets.push_back(following_lanelet); } - if (has_target_lane_id) { - // add path point with target lane id - // (lanelet with target lane id is exit of private road) - private_path.points.emplace_back(path.points.at(i)); - prev_has_target_lane_id = true; + } + + lanelet::ConstLanelets attention_lanelets; + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(sibling_lanelets, conflicting_lanelet)) { continue; } - if (prev_has_target_lane_id) { - // extend path to the front - private_path.points.emplace_back(path.points.at(i)); - sum_dist += tier4_autoware_utils::calcDistance2d( - path.points.at(i).point.pose, path.points.at(i + 1).point.pose); - if (sum_dist > extend_length) { - break; - } - } + attention_lanelets.push_back(conflicting_lanelet); } - - std::reverse(private_path.points.begin(), private_path.points.end()); - return private_path; + return attention_lanelets; } + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index fab0303640700..a44b99c97457d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,10 +15,7 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include "scene_intersection.hpp" - #include -#include #include #include @@ -27,9 +24,6 @@ #include #include -#include -#include - #include #include #include @@ -79,17 +73,15 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface motion_utils::VirtualWalls createVirtualWalls() override; const std::set & getAssociativeIds() const { return associative_ids_; } + lanelet::ConstLanelets getAttentionLanelets() const; private: const int64_t lane_id_; const std::set associative_ids_; - autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length); - // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional first_conflicting_lanelet_; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 1c7e366347fec..dfdfb01fb2234 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -14,59 +14,36 @@ #include "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 +#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 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) @@ -83,7 +60,7 @@ static std::optional getDuplicatedPointIdx( return std::nullopt; } -static std::optional insertPointIndex( +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) @@ -155,67 +132,9 @@ std::optional> findLaneIdsInterval( return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; } -/** - * @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. - */ -static std::optional getStopLineIndexFromMap( - const InterpolatedPathInfo & interpolated_path_info, - const std::shared_ptr & planner_data) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - lanelet::ConstLanelet lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get( - interpolated_path_info.lane_id); - const auto road_markings = 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 std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, +std::optional getFirstPointInsidePolygonByFootprint( + 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; @@ -235,11 +154,11 @@ static std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } -static std::optional> getFirstPointInsidePolygonsByFootprint( const std::vector & polygons, - const 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; @@ -263,194 +182,6 @@ getFirstPointInsidePolygonsByFootprint( return std::nullopt; } -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - 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 detection_area - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_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())) { - // TODO(Mamoru Sobue): 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, planner_data); - 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_detection_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 detection 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_detection_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)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; - const bool first_attention_stopline_valid = true; - - // (4) 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 pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line - int stuck_stopline_ip_int = 0; - bool stuck_stopline_valid = true; - if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection 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)); - - 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 occlusion_peeking_stopline{0}; - size_t 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}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.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 = 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 (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; - intersection_stoplines.occlusion_wo_tl_pass_judge_line = - intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stoplines; -} - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -489,105 +220,7 @@ std::optional getFirstPointInsidePolygon( return std::nullopt; } -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 generateStuckStopLine( - const lanelet::CompoundPolygon3d & conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - 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 auto lane_interval_ip_start = std::get<0>(lane_interval_ip); - size_t stuck_stopline_idx_ip = 0; - if (use_stuck_stopline) { - stuck_stopline_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stopline_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, conflicting_area); - if (!stuck_stopline_idx_ip_opt) { - return std::nullopt; - } - stuck_stopline_idx_ip = stuck_stopline_idx_ip_opt.value(); - } - - 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); - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stopline_idx_ip) - 1 - stopline_margin_idx_dist - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - return insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - -/** - * @param pair lanelets and the vector of original lanelets in topological order (not reversed as - *in generateDetectionLaneDivisions()) - **/ -static std::pair> +std::pair> mergeLaneletsByTopologicalSort( const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr) @@ -682,207 +315,9 @@ mergeLaneletsByTopologicalSort( return {merged, originals}; } -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle) -{ - const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - - // 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 auto turn_direction = getTurnDirection(ll); - 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] = - 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; -} - bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -892,8 +327,8 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -902,81 +337,13 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -/* -static std::vector getAllAdjacentLanelets( - const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) -{ - std::set results; - - results.insert(lane.id()); - - auto it = routing_graph->adjacentRight(lane); - // take all lane on the right side - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentRight(it.get()); - } - // take all lane on the left side - it = routing_graph->adjacentLeft(lane); - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentLeft(it.get()); - - } - return std::vector(results.begin(), results.end()); -} -*/ - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( - lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, - lanelet::ConstLanelet lane) -{ - // some of the intersections are not well-formed, and "adjacent" turning - // lanelets are not sharing the LineStrings - const std::string turn_direction = getTurnDirection(lane); - if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") - return {}; - - std::set previous_lanelet_ids; - for (auto && previous_lanelet : routing_graph->previous(lane)) { - previous_lanelet_ids.insert(previous_lanelet.id()); - } - - std::set besides_previous_lanelet_ids; - for (auto && previous_lanelet_id : previous_lanelet_ids) { - lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); - for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { - besides_previous_lanelet_ids.insert(beside_lanelet); - } - } - - std::set following_turning_lanelets; - following_turning_lanelets.insert(lane.id()); - for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { - lanelet::ConstLanelet besides_previous_lanelet = - map->laneletLayer.get(besides_previous_lanelet_id); - for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { - // if this has {"turn_direction", "${turn_direction}"}, take this - if (getTurnDirection(following_lanelet) == turn_direction) - following_turning_lanelets.insert(following_lanelet.id()); - } - } - lanelet::ConstLanelets ret{}; - for (auto && id : following_turning_lanelets) { - ret.push_back(map->laneletLayer.get(id)); - } - return ret; -} -*/ - -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); if (area_id_str == "else") return std::nullopt; - const int area_id = std::atoi(area_id_str.c_str()); + const lanelet::Id area_id = std::atoi(area_id_str.c_str()); const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); Polygon2d poly{}; for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); @@ -993,117 +360,12 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos) -{ - 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_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { - // the info of this traffic light is not available - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_it->second; - 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; -} - -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 generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double 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] = - 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; -} - -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; } @@ -1115,7 +377,6 @@ std::optional generateInterpolatedPath( return interpolated_path_info; } -// from here geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) { @@ -1133,503 +394,14 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -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; -} - -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data) -{ - 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 > stuck_vehicle_vel_thr) { - 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); - return true; - } - } - return false; -} - -bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, - const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) -{ - const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area); - Polygon2d first_attention_area_poly; - for (const auto & p : first_attention_area_2d) { - first_attention_area_poly.outer().emplace_back(p.x(), p.y()); - } - - 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 > stuck_vehicle_vel_thr) { - continue; // not stop vehicle - } - - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - - // check if the object is too close to the ego path - if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { - continue; - } - - // check if the footprint is in the stuck detect area - const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly); - if (is_in_stuck_area && debug_data) { - debug_data->yield_stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - Polygon2d polygon{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return polygon; - } - - 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 polygon; - } - - for (const auto & p : target_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - - polygon.outer().emplace_back(polygon.outer().front()); - bg::correct(polygon); - - return polygon; -} - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle) -{ - 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 cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, 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); - } - } - } - } -} - -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - 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 = 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; - } - 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; -} - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) -{ - const auto lane_id = assigned_lanelet.id(); - const auto intersection_first_itr = - std::find_if(path.points.cbegin(), path.points.cend(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - if ( - intersection_first_itr == path.points.begin() || intersection_first_itr == path.points.end()) { - return 0.0; - } - const auto dst_idx = std::distance(path.points.begin(), intersection_first_itr) - 1; - - if (closest_idx > static_cast(dst_idx)) { - return 0.0; - } - - double distance = std::abs(motion_utils::calcSignedArcLength(path.points, closest_idx, dst_idx)); - const auto & lane_first_point = assigned_lanelet.centerline2d().front(); - distance += std::hypot( - path.points.at(dst_idx).point.pose.position.x - lane_first_point.x(), - path.points.at(dst_idx).point.pose.position.y - lane_first_point.y()); - return distance; -} - -void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - 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_) { - 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); - } - } -} - -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 -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; - 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 generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - 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) -{ - 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 = 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() - ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : 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; -} - -void TargetObject::calc_dist_to_stopline() +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) { - if (!attention_lanelet || !stopline) { - return; + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); + return polys; } -} // namespace util -} // namespace behavior_velocity_planner +} // 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 8d0e673fc931e..f103191b2dc6f 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,109 +15,100 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "scene_intersection.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 -namespace behavior_velocity_planner -{ -namespace util +namespace behavior_velocity_planner::util { -std::optional insertPoint( + +/** + * @fn + * @brief insert a new pose to the path and return its index + * @return if insertion was successful return the inserted point index + */ +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); +/** + * @fn + * @brief check if a PathPointWithLaneId contains any of the given lane ids + */ bool hasLaneIds( const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); -std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief get objective polygons for detection area + * @fn + * @brief find the first contiguous interval of the path points that contains the specified lane ids + * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is + * found, returns the pair (start-1, end) */ -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle); +std::optional> findLaneIdsInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief Generate a stop line for stuck vehicle - * @param conflicting_areas used to generate stop line for stuck vehicle - * @param original_path ego-car lane - * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car - * lane) - " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + * @fn + * @brief return the index of the first point which is inside the given polygon + * @param[in] lane_interval the interval of the path points on the intersection + * @param[in] search_forward flag for search direction */ -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); /** + * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose - * @param path path - * @param closest_idx ego's closest index on the path - * @param current_pose ego's exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); +/** + * @fn + * @brief check if ego is before the target_idx. If the index is same, compare the exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose + * @return true if ego is over the target_idx + */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( -lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, -lanelet::ConstLanelet lane); -*/ + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); +/** + * @fn + * @brief check if the given lane has related traffic light + */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds); - -std::optional generateInterpolatedPath( +/** + * @fn + * @brief interpolate PathWithLaneId + */ +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); @@ -125,57 +116,40 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data); - -bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, - const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data); - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle); - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, - const double time_thr); +/** + * @fn + * @brief this function sorts the set of lanelets topologically using topological sort and merges + * the lanelets from each root to each end. each branch is merged and returned with the original + * lanelets + * @param[in] lanelets the set of lanelets + * @param[in] routing_graph_ptr the routing graph + * @return the pair of merged lanelets and their corresponding original lanelets + */ +std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr); -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); - -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval); - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - 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); - -} // namespace util -} // namespace behavior_velocity_planner +/** + * @fn + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygon + */ +std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + +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_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp deleted file mode 100644 index 3c7ba3041b0bd..0000000000000 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ /dev/null @@ -1,210 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UTIL_TYPE_HPP_ -#define UTIL_TYPE_HPP_ - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace behavior_velocity_planner::util -{ - -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 stuck_vehicle_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 InterpolatedPathInfo -{ - autoware_auto_planning_msgs::msg::PathWithLaneId path; - double ds{0.0}; - lanelet::Id lane_id{0}; - std::set associative_lane_ids{}; - std::optional> lane_id_interval{std::nullopt}; -}; - -struct IntersectionLanelets -{ -public: - void update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); - 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 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_; - } - - lanelet::ConstLanelets attention_; // topologically merged lanelets - std::vector> - attention_stoplines_; // the stop lines for each attention_ lanelets - lanelet::ConstLanelets attention_non_preceding_; - std::vector> - attention_non_preceding_stoplines_; // the stop lines for each attention_non_preceding_ - // lanelets - lanelet::ConstLanelets conflicting_; - lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets - std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets - std::vector attention_area_; // topologically merged lanelets - std::vector attention_non_preceding_area_; - std::vector conflicting_area_; - std::vector adjacent_area_; - std::vector - occlusion_attention_area_; // topologically merged lanelets - // the first area intersecting with the path - // even if lane change/re-routing happened on the intersection, these areas area are supposed to - // be invariant under the 'associative' lanes. - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - bool is_prioritized_ = false; -}; - -struct IntersectionStopLines -{ - // NOTE: for baselink - size_t closest_idx{0}; - // NOTE: null if path does not conflict with first_conflicting_area - std::optional stuck_stopline{std::nullopt}; - // NOTE: null if path is over map stopline OR its value is calculated negative - std::optional default_stopline{std::nullopt}; - // NOTE: null if the index is calculated negative - std::optional first_attention_stopline{std::nullopt}; - // NOTE: null if footprints do not change from outside to inside of detection area - std::optional occlusion_peeking_stopline{std::nullopt}; - // if the value is calculated negative, its value is 0 - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -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 TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - -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 -}; -} // namespace behavior_velocity_planner::util - -#endif // UTIL_TYPE_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_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 47d4985ae643e..e049d02ffe9b5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -230,6 +230,8 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( } const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); if (obj_v > planner_param_.stuck_vehicle_vel_thr) { + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::GREEN); continue; // not stop vehicle } // check if the footprint is in the stuck detect area @@ -237,6 +239,8 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); for (const auto & p : obj_footprint.outer()) { geometry_msgs::msg::Point point; point.x = p.x(); diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index 69ad30e32dfdc..9f580149be7ae 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -123,7 +123,7 @@ bool isCollisionFree( return true; } -boost::optional generateOcclusionPolygon( +std::optional generateOcclusionPolygon( const Polygon2d & occupancy_poly, const Point2d & origin, const Point2d & min_theta_pos, const Point2d & max_theta_pos, const double ray_max_length = 100.0) { @@ -157,7 +157,7 @@ boost::optional generateOcclusionPolygon( occlusion_poly.outer().emplace_back(max_intersections.front()); } //! case outside detection area - if (occlusion_poly.outer().size() == 2) return boost::none; + if (occlusion_poly.outer().size() == 2) return std::nullopt; boost::geometry::correct(occlusion_poly); Polygon2d hull_poly; boost::geometry::convex_hull(occlusion_poly, hull_poly); @@ -200,7 +200,7 @@ std::pair calcEdgePoint(const Polygon2d & foot_print, const Poin return std::make_pair(min_idx, max_idx); } -boost::optional generateOccupiedPolygon( +std::optional generateOccupiedPolygon( const Polygon2d & occupancy_poly, const Polygon2d & foot_print, const Point & position) { Point2d origin = {position.x, position.y}; @@ -275,9 +275,9 @@ void generateOccupiedImage( for (const auto & foot_print : moving_vehicle_foot_prints) { // calculate occlusion polygon from moving vehicle const auto polys = generateOccupiedPolygon(occupancy_poly, foot_print, scan_origin); - if (polys == boost::none) continue; + if (polys == std::nullopt) continue; // transform to cv point and stuck it to cv polygon - for (const auto & p : polys.get().outer()) { + for (const auto & p : polys.value().outer()) { const Point transformed_geom_pt = transformFromMap2Grid(geom_tf_map2grid, p); cv_polygon.emplace_back( toCVPoint(transformed_geom_pt, width, height, occupancy_grid.info.resolution)); diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 849e16eb0834c..c9dbe13474b06 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -99,7 +99,7 @@ void findOcclusionSpots( bool isCollisionFree( const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2, const double radius); -boost::optional generateOccupiedPolygon( +std::optional generateOccupiedPolygon( const Polygon2d & occupancy_poly, const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints, const Point & position); //!< @brief generate occupied polygon from foot print diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index fa8be96c02859..47e643241740d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -406,11 +406,11 @@ bool generatePossibleCollisionsFromGridMap( const auto pc = generateOneNotableCollisionFromOcclusionSpot( grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, debug_data); - if (pc == boost::none) continue; - const double lateral_distance = std::abs(pc.get().arc_lane_dist_at_collision.distance); + if (pc) continue; + const double lateral_distance = std::abs(pc.value().arc_lane_dist_at_collision.distance); if (lateral_distance > distance_lower_bound) continue; distance_lower_bound = lateral_distance; - possible_collisions.emplace_back(pc.get()); + possible_collisions.emplace_back(pc.value()); } return !possible_collisions.empty(); } @@ -423,7 +423,7 @@ bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d return false; } -boost::optional generateOneNotableCollisionFromOcclusionSpot( +std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data) @@ -473,7 +473,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS has_collision = true; } if (has_collision) return candidate; - return boost::none; + return std::nullopt; } } // namespace occlusion_spot_utils diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index cc07461864c16..ec58e38dec45a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -30,8 +30,6 @@ #include #include -#include - #include #include #include @@ -67,7 +65,7 @@ using lanelet::ConstLineString2d; using lanelet::LaneletMapPtr; using lanelet::geometry::fromArcCoordinates; using lanelet::geometry::toArcCoordinates; -using DetectionAreaIdx = boost::optional>; +using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; namespace occlusion_spot_utils @@ -240,7 +238,7 @@ void calcSlowDownPointsForPossibleCollision( const int closest_idx, const PathWithLaneId & path, const double offset, std::vector & possible_collisions); //!< @brief convert a set of occlusion spots found on detection_area slice -boost::optional generateOneNotableCollisionFromOcclusionSpot( +std::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data); diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index aa51c38b55742..b31506918a2db 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -2,6 +2,7 @@ ros__parameters: forward_path_length: 1000.0 backward_path_length: 5.0 + behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 max_accel: -2.8 max_jerk: -5.0 diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 22220ccd182a1..c1631fdb739de 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -26,6 +26,7 @@ + @@ -69,6 +70,7 @@ + diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 7c0036b7812fa..9fa5634c6dd65 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -138,6 +138,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Parameters forward_path_length_ = declare_parameter("forward_path_length"); backward_path_length_ = declare_parameter("backward_path_length"); + behavior_output_path_interval_ = declare_parameter("behavior_output_path_interval"); planner_data_.stop_line_extend_length = declare_parameter("stop_line_extend_length"); // nearest search @@ -147,6 +148,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { + // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. + if (name == "") { + break; + } planner_manager_.launchScenePlugin(*this, name); } @@ -318,11 +323,33 @@ 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; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + 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 { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } } } @@ -405,7 +432,8 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + const auto interpolated_path_msg = + interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point output_path_msg = filterStopPathPoint(interpolated_path_msg); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index ded1d08700a1d..a345f1200f721 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -102,6 +102,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // parameter double forward_path_length_; double backward_path_length_; + double behavior_output_path_interval_; // member PlannerData planner_data_; diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 491df4c7a8766..239abbde27609 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2ea9f6ed2648b..606e41ad4b1d1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -76,7 +76,10 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -125,12 +128,20 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal(const int id) const + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation = false) const { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; if (traffic_light_id_map.count(id) == 0) { - return {}; + return std::nullopt; } - return std::make_shared(traffic_light_id_map.at(id)); + return std::make_optional(traffic_light_id_map.at(id)); } }; } // namespace behavior_velocity_planner 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 ced2e267cc025..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 @@ -19,8 +19,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -47,14 +49,30 @@ namespace behavior_velocity_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; +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; using tier4_rtc_msgs::msg::Module; using unique_identifier_msgs::msg::UUID; +struct ObjectOfInterest +{ + geometry_msgs::msg::Pose pose; + autoware_auto_perception_msgs::msg::Shape shape; + ColorName color; + ObjectOfInterest( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + : pose(pose), shape(shape), color(color_name) + { + } +}; + class SceneModuleInterface { public: @@ -94,6 +112,8 @@ class SceneModuleInterface void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } + std::vector getObjectsOfInterestData() const { return objects_of_interest_; } + void clearObjectsOfInterestData() { objects_of_interest_.clear(); } protected: const int64_t module_id_; @@ -107,6 +127,7 @@ class SceneModuleInterface std::optional infrastructure_command_; std::optional first_stop_path_point_index_; VelocityFactorInterface velocity_factor_; + std::vector objects_of_interest_; void setSafe(const bool safe) { @@ -118,6 +139,13 @@ class SceneModuleInterface void setDistance(const double distance) { distance_ = distance; } void syncActivation() { setActivation(isSafe()); } + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + { + objects_of_interest_.emplace_back(pose, shape, color_name); + } + size_t findEgoSegmentIndex( const std::vector & points) const; }; @@ -200,6 +228,8 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface RTCInterface rtc_interface_; std::unordered_map map_uuid_; + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; + virtual void sendRTC(const Time & stamp); virtual void setActivation(); @@ -220,7 +250,24 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void removeUUID(const int64_t & module_id); + 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_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 591ef8667b81b..667c915ac1d7f 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -28,8 +28,7 @@ bool splineInterpolate( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, - const double interval = 1.0); + const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval); autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( const autoware_auto_planning_msgs::msg::Path & path); autoware_auto_planning_msgs::msg::Path filterStopPathPoint( diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index c86272cff1c2e..2184be4fbd2fe 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -35,11 +35,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path); -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory); - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 187720ff92a15..784b7cabfe9ad 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -32,6 +32,7 @@ motion_utils motion_velocity_smoother nav_msgs + objects_of_interest_marker_interface pcl_conversions rclcpp rclcpp_components diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 49a52fcd60df2..362493005eb17 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -174,7 +174,7 @@ void SceneModuleManagerInterface::deleteExpiredModules( void SceneModuleManagerInterface::registerModule( const std::shared_ptr & scene_module) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); scene_modules_.insert(scene_module); @@ -183,7 +183,7 @@ void SceneModuleManagerInterface::registerModule( void SceneModuleManagerInterface::unregisterModule( const std::shared_ptr & scene_module) { - RCLCPP_INFO( + RCLCPP_DEBUG( logger_, "unregister task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.erase(scene_module->getModuleId()); @@ -192,7 +192,9 @@ void SceneModuleManagerInterface::unregisterModule( SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc) -: SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) +: SceneModuleManagerInterface(node, module_name), + rtc_interface_(&node, module_name, enable_rtc), + objects_of_interest_marker_interface_(&node, module_name) { } @@ -202,6 +204,7 @@ void SceneModuleManagerInterfaceWithRTC::plan( setActivation(); modifyPathVelocity(path); sendRTC(path->header.stamp); + publishObjectsOfInterestMarker(); } void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) @@ -244,6 +247,19 @@ void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) } } +void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() +{ + for (const auto & scene_module : scene_modules_) { + const auto objects = scene_module->getObjectsOfInterestData(); + for (const auto & obj : objects) { + objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); + } + scene_module->clearObjectsOfInterestData(); + } + + objects_of_interest_marker_interface_.publishMarkerArray(); +} + void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 7fe8612cc6858..314281a954b49 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -13,6 +13,8 @@ // limitations under the License. // #include +#include "motion_utils/trajectory/conversion.hpp" + #include #include #include @@ -41,41 +43,6 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) -{ - TrajectoryPoints tps; - for (const auto & p : path.points) { - TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - tps.emplace_back(tp); - } - return tps; -} -PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) -{ - PathWithLaneId path; - for (const auto & p : trajectory) { - PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - path.points.emplace_back(pp); - } - return path; -} - -Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); - - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} - //! smooth path point with lane id starts from ego position on path to the path end bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, @@ -87,7 +54,9 @@ bool smoothPath( const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; - auto trajectory = convertPathToTrajectoryPoints(in_path); + auto trajectory = + motion_utils::convertToTrajectoryPoints( + in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = @@ -117,7 +86,7 @@ bool smoothPath( motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } - out_path = convertTrajectoryPointsToPath(traj_smoothed); + out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } 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 ea63462b32d84..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,7 +3,8 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data - 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 @@ -11,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: @@ -33,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 7bd27fca3407c..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 @@ -20,6 +21,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + behavior_velocity_crosswalk_module behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 91656d542ea8e..b28725a92628e 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -35,14 +35,17 @@ class DebugValues public: enum class TYPE { CALCULATION_TIME = 0, - CALCULATION_TIME_COLLISION_CHECK = 1, - LATERAL_DIST = 2, - LONGITUDINAL_DIST_OBSTACLE = 3, - LONGITUDINAL_DIST_COLLISION = 4, - COLLISION_POS_FROM_EGO_FRONT = 5, - STOP_DISTANCE = 6, - NUM_OBSTACLES = 7, - LATERAL_PASS_DIST = 8, + CALCULATION_TIME_PATH_PROCESSING = 1, + CALCULATION_TIME_OBSTACLE_CREATION = 2, + CALCULATION_TIME_COLLISION_CHECK = 3, + CALCULATION_TIME_PATH_PLANNING = 4, + LATERAL_DIST = 5, + LONGITUDINAL_DIST_OBSTACLE = 6, + LONGITUDINAL_DIST_COLLISION = 7, + COLLISION_POS_FROM_EGO_FRONT = 8, + STOP_DISTANCE = 9, + NUM_OBSTACLES = 10, + LATERAL_PASS_DIST = 11, SIZE, // this is the number of enum elements }; diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 09c87ed81cf38..f2ef366cbe121 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) auto & p = planner_param_.run_out; p.detection_method = getOrDeclareParameter(node, ns + ".detection_method"); p.use_partition_lanelet = getOrDeclareParameter(node, ns + ".use_partition_lanelet"); + p.suppress_on_crosswalk = getOrDeclareParameter(node, ns + ".suppress_on_crosswalk"); p.specify_decel_jerk = getOrDeclareParameter(node, ns + ".specify_decel_jerk"); p.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); p.passing_margin = getOrDeclareParameter(node, ns + ".passing_margin"); @@ -104,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 2fa8cf241ee74..50b69fc6139c9 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -14,6 +14,7 @@ #include "scene.hpp" +#include "behavior_velocity_crosswalk_module/util.hpp" #include "path_utils.hpp" #include @@ -23,6 +24,10 @@ #include #include +#include + +#include +#include #include #include @@ -41,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); @@ -60,7 +65,7 @@ bool RunOutModule::modifyPathVelocity( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { // timer starts - const auto t1_modify_path = std::chrono::system_clock::now(); + const auto t_start = std::chrono::system_clock::now(); // set planner data const auto current_vel = planner_data_->current_velocity->twist.linear.x; @@ -70,20 +75,27 @@ bool RunOutModule::modifyPathVelocity( // set height of debug data debug_ptr_->setHeight(current_pose.position.z); - // smooth velocity of the path to calculate time to collision accurately - PathWithLaneId smoothed_path; - if (!smoothPath(*path, smoothed_path, planner_data_)) { - return true; - } - // extend path to consider obstacles after the goal - const auto extended_smoothed_path = - run_out_utils::extendPath(smoothed_path, planner_param_.vehicle_param.base_to_front); + const auto extended_path = + run_out_utils::extendPath(*path, planner_param_.vehicle_param.base_to_front); // trim path ahead of the base_link to make calculation easier const double trim_distance = planner_param_.run_out.detection_distance; - const auto trim_smoothed_path = - run_out_utils::trimPathFromSelfPose(extended_smoothed_path, current_pose, trim_distance); + const auto trim_path = + run_out_utils::trimPathFromSelfPose(extended_path, current_pose, trim_distance); + + // smooth velocity of the path to calculate time to collision accurately + PathWithLaneId extended_smoothed_path; + if (!smoothPath(trim_path, extended_smoothed_path, planner_data_)) { + return true; + } + + // record time for path processing + const auto t_path_processing = std::chrono::system_clock::now(); + const auto elapsed_path_processing = + std::chrono::duration_cast(t_path_processing - t_start); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_PATH_PROCESSING, elapsed_path_processing.count() / 1000.0); // create abstracted dynamic obstacles from objects or points dynamic_obstacle_creator_->setData(*planner_data_, planner_param_, *path, extended_smoothed_path); @@ -92,18 +104,30 @@ bool RunOutModule::modifyPathVelocity( // extract obstacles using lanelet information const auto partition_excluded_obstacles = - excludeObstaclesOutSideOfPartition(dynamic_obstacles, trim_smoothed_path, current_pose); + excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose); - // timer starts - const auto t1_collision_check = std::chrono::system_clock::now(); + // record time for obstacle creation + const auto t_obstacle_creation = std::chrono::system_clock::now(); + const auto elapsed_obstacle_creation = + std::chrono::duration_cast(t_obstacle_creation - t_path_processing); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_OBSTACLE_CREATION, + elapsed_obstacle_creation.count() / 1000.0); // detect collision with dynamic obstacles using velocity planning of ego - const auto dynamic_obstacle = detectCollision(partition_excluded_obstacles, trim_smoothed_path); - - // timer ends - const auto t2_collision_check = std::chrono::system_clock::now(); + const auto crosswalk_lanelets = planner_param_.run_out.suppress_on_crosswalk + ? getCrosswalksOnPath( + planner_data_->current_odometry->pose, *path, + planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->route_handler_->getOverallGraphPtr()) + : std::vector>(); + const auto dynamic_obstacle = + detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets); + + // record time for collision check + const auto t_collision_check = std::chrono::system_clock::now(); const auto elapsed_collision_check = - std::chrono::duration_cast(t2_collision_check - t1_collision_check); + std::chrono::duration_cast(t_collision_check - t_obstacle_creation); debug_ptr_->setDebugValues( DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0); @@ -112,7 +136,7 @@ bool RunOutModule::modifyPathVelocity( // after a certain amount of time has elapsed since the ego stopped, // approach the obstacle with slow velocity insertVelocityForState( - dynamic_obstacle, *planner_data_, planner_param_, trim_smoothed_path, *path); + dynamic_obstacle, *planner_data_, planner_param_, extended_smoothed_path, *path); } else { // just insert zero velocity for stopping insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path); @@ -124,20 +148,26 @@ bool RunOutModule::modifyPathVelocity( } publishDebugValue( - trim_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); + extended_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); - // timer ends - const auto t2_modify_path = std::chrono::system_clock::now(); - const auto elapsed_modify_path = - std::chrono::duration_cast(t2_modify_path - t1_modify_path); + // record time for collision check + const auto t_path_planning = std::chrono::system_clock::now(); + const auto elapsed_path_planning = + std::chrono::duration_cast(t_path_planning - t_collision_check); debug_ptr_->setDebugValues( - DebugValues::TYPE::CALCULATION_TIME, elapsed_modify_path.count() / 1000.0); + DebugValues::TYPE::CALCULATION_TIME_PATH_PLANNING, elapsed_path_planning.count() / 1000.0); + + // record time for the function + const auto t_end = std::chrono::system_clock::now(); + const auto elapsed_all = std::chrono::duration_cast(t_end - t_start); + debug_ptr_->setDebugValues(DebugValues::TYPE::CALCULATION_TIME, elapsed_all.count() / 1000.0); return true; } std::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const std::vector> & crosswalk_lanelets) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -171,7 +201,7 @@ std::optional RunOutModule::detectCollision( debug_ptr_->pushTravelTimeTexts(travel_time, p2.pose, /* lateral_offset */ 3.0); auto obstacles_collision = - checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time); + checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time, crosswalk_lanelets); if (obstacles_collision.empty()) { continue; } @@ -291,7 +321,8 @@ std::vector RunOutModule::createVehiclePolygon( std::vector RunOutModule::checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time) const + std::vector poly, const float travel_time, + const std::vector> & crosswalk_lanelets) const { const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); @@ -320,8 +351,8 @@ std::vector RunOutModule::checkCollisionWithObstacles( *predicted_obstacle_pose_min_vel, *predicted_obstacle_pose_max_vel}; std::vector collision_points; - const bool collision_detected = - checkCollisionWithShape(bg_poly_vehicle, pose_with_range, obstacle.shape, collision_points); + const bool collision_detected = checkCollisionWithShape( + bg_poly_vehicle, pose_with_range, obstacle.shape, crosswalk_lanelets, collision_points); if (!collision_detected) { continue; @@ -380,6 +411,7 @@ std::optional RunOutModule::calcPredictedObstaclePose( bool RunOutModule::checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + const std::vector> & crosswalk_lanelets, std::vector & collision_points) const { bool collision_detected = false; @@ -402,6 +434,23 @@ bool RunOutModule::checkCollisionWithShape( break; } + if (!collision_points.empty()) { + for (const auto & crosswalk : crosswalk_lanelets) { + const auto poly = crosswalk.second.polygon2d().basicPolygon(); + for (auto it = collision_points.begin(); it != collision_points.end();) { + if (boost::geometry::within(lanelet::BasicPoint2d(it->x, it->y), poly)) { + it = collision_points.erase(it); + } else { + ++it; + } + } + if (collision_points.empty()) { + break; + } + } + collision_detected = !collision_points.empty(); + } + return collision_detected; } @@ -566,16 +615,25 @@ std::optional RunOutModule::calcStopPoint( // vehicle have to decelerate if there is not enough distance with deceleration_jerk const bool deceleration_needed = *stop_dist > dist_to_collision - planner_param_.run_out.stop_margin; - // avoid acceleration when ego is decelerating - // TODO(Tomohito Ando): replace with more appropriate method - constexpr float epsilon = 1.0e-2; - constexpr float stopping_vel_mps = 2.5 / 3.6; - const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; - if (!deceleration_needed && !is_stopping) { + const auto & detection_method = planner_param_.run_out.detection_method; + + if (!deceleration_needed && detection_method == "Object") { debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); return {}; } + // If the detection method assumes running out, avoid acceleration when the ego is decelerating. + // TODO(Tomohito Ando): replace with more appropriate way + if (!deceleration_needed && detection_method != "Object") { + constexpr float epsilon = 1.0e-2; + constexpr float stopping_vel_mps = 2.5 / 3.6; + const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; + if (!is_stopping) { + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); + return {}; + } + } + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index b1c49189267d4..def90f036c440 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -67,7 +68,8 @@ class RunOutModule : public SceneModuleInterface Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; std::optional detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const std::vector> & crosswalk_lanelets); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -77,7 +79,8 @@ class RunOutModule : public SceneModuleInterface std::vector checkCollisionWithObstacles( const std::vector & dynamic_obstacles, - std::vector poly, const float travel_time) const; + std::vector poly, const float travel_time, + const std::vector> & crosswalk_lanelets) const; std::optional findNearestCollisionObstacle( const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, @@ -89,6 +92,7 @@ class RunOutModule : public SceneModuleInterface bool checkCollisionWithShape( const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + const std::vector> & crosswalk_lanelets, std::vector & collision_points) const; bool checkCollisionWithCylinder( diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 6afe451f72f73..c4976a119dd00 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -54,6 +54,7 @@ struct RunOutParam { std::string detection_method; bool use_partition_lanelet; + bool suppress_on_crosswalk; bool specify_decel_jerk; double stop_margin; double passing_margin; @@ -84,13 +85,6 @@ struct MandatoryArea float decel_jerk; }; -struct ApproachingParam -{ - bool enable; - float margin; - float limit_vel_kmph; -}; - struct StateParam { float stop_thresh; @@ -99,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; @@ -142,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/README.md b/planning/behavior_velocity_traffic_light_module/README.md index 866ab87070369..e85a171495e38 100644 --- a/planning/behavior_velocity_traffic_light_module/README.md +++ b/planning/behavior_velocity_traffic_light_module/README.md @@ -19,7 +19,13 @@ This module is activated when there is traffic light in ego lane. 1. Obtains a traffic light mapped to the route and a stop line correspond to the traffic light from a map information. -2. Uses the highest reliability one of the traffic light recognition result and if the color of that was red, generates a stop point. + - If a corresponding traffic light signal have never been found, it treats as a signal to pass. + + - If a corresponding traffic light signal is found but timed out, it treats as a signal to stop. + +2. Uses the highest reliability one of the traffic light recognition result and if the color of that was not green or corresponding arrow signal, generates a stop point. + + - If an elapsed time to receive stop signal is less than `stop_time_hysteresis`, it treats as a signal to pass. This feature is to prevent chattering. 3. When vehicle current velocity is @@ -63,12 +69,13 @@ This module is activated when there is traffic light in ego lane. #### Module Parameters -| Parameter | Type | Description | -| -------------------- | ------ | ----------------------------------------------- | -| `stop_margin` | double | [m] margin before stop point | -| `tl_state_timeout` | double | [s] time out for detected traffic light result. | -| `yellow_lamp_period` | double | [s] time for yellow lamp | -| `enable_pass_judge` | bool | [-] whether to use pass judge | +| Parameter | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------- | +| `stop_margin` | double | [m] margin before stop point | +| `tl_state_timeout` | double | [s] time out for detected traffic light result. | +| `stop_time_hysteresis` | double | [s] time threshold to decide stop planning for chattering prevention | +| `yellow_lamp_period` | double | [s] time for yellow lamp | +| `enable_pass_judge` | bool | [-] whether to use pass judge | #### Flowchart @@ -91,7 +98,7 @@ if (state is APPROACH) then (yes) :change state to GO_OUT; stop elseif (no stop signal) then (yes) - :change previous state to STOP; + :change previous state to PASS; stop elseif (not pass through) then (yes) :insert stop pose; diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index a837ae1b46b9b..23746a61b6043 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -3,6 +3,7 @@ traffic_light: stop_margin: 0.0 tl_state_timeout: 1.0 + stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 enable_pass_judge: true 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_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index c658f0890b986..33bb471e5920c 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -24,7 +24,6 @@ eigen geometry_msgs lanelet2_extension - libboost-dev motion_utils pluginlib rclcpp @@ -34,6 +33,7 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs + traffic_light_utils visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index 9a0ba4f37c3c0..b3dedaa29d6ad 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -26,8 +26,6 @@ namespace behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; - visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index a23edefff52ab..89817f3342dbd 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include namespace behavior_velocity_planner { @@ -32,12 +31,13 @@ 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"); planner_param_.tl_state_timeout = getOrDeclareParameter(node, ns + ".tl_state_timeout"); + planner_param_.stop_time_hysteresis = + getOrDeclareParameter(node, ns + ".stop_time_hysteresis"); planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index c36c6af1128ce..87213d8a5ed3f 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -26,6 +26,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -57,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; - boost::optional first_ref_stop_path_point_index_; + std::optional first_ref_stop_path_point_index_; }; class TrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 90a547adeb930..9982ec34c4bca 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -16,10 +16,10 @@ #include #include +#include #include #include -#include // To be replaced by std::optional in C++17 #include @@ -31,9 +31,7 @@ #include #include -#include #include -#include #include namespace behavior_velocity_planner @@ -52,14 +50,14 @@ bool getBackwardPointFromBasePoint( return true; } -boost::optional findNearestCollisionPoint( +std::optional findNearestCollisionPoint( const LineString2d & line1, const LineString2d & line2, const Point2d & origin) { std::vector collision_points; bg::intersection(line1, line2, collision_points); if (collision_points.empty()) { - return boost::none; + return std::nullopt; } // check nearest collision point @@ -220,15 +218,35 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Move to go out state if ego vehicle over deadline. constexpr double signed_deadline_length = -2.0; if (signed_arc_length_to_stop_point < signed_deadline_length) { - RCLCPP_INFO(logger_, "APPROACH -> GO_OUT"); + RCLCPP_DEBUG(logger_, "APPROACH -> GO_OUT"); state_ = State::GO_OUT; + stop_signal_received_time_ptr_.reset(); return true; } first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal()); + const bool is_stop_signal = isStopSignal(); + + // Update stop signal received time + if (is_stop_signal) { + if (!stop_signal_received_time_ptr_) { + stop_signal_received_time_ptr_ = std::make_unique