diff --git a/.cspell-partial.json b/.cspell-partial.json index 209882ddc2f99..45b85e779c39d 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -1,14 +1,5 @@ { - "ignorePaths": [ - "**/common/**", - "**/control/**", - "**/evaluator/**", - "**/launch/**", - "**/perception/**", - "**/planning/**", - "**/sensing/**", - "**/system/**" - ], + "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"], "ignoreRegExpList": [], "words": [] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ca03cd9a56adc..fb6df15ab7679 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,27 +2,29 @@ ### 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_auto_common/** opensource@apex.ai +common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp -common/autoware_auto_tf2/** jit.ray.c@gmail.com +common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp -common/autoware_testing/** adam.dabrowski@robotec.ai +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/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp -common/fake_test_node/** opensource@apex.ai +common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp +common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@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/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp +common/perception_utils/** mingyu.li@tier4.jp 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 common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -45,15 +47,17 @@ common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp 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 +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/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/lane_departure_checker/** kyoichi.sugahara@tier4.jp +control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @@ -64,10 +68,9 @@ control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@ti control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp -evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai -evaluator/localization_evaluator/** dominik.jargot@robotec.ai +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/map4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryohei.sasaki@map.jp ryu.yamamoto@tier4.jp yamato.ando@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_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 @@ -78,21 +81,23 @@ 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/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ekf_localizer/** koji.minoda@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp -localization/gyro_odometer/** yamato.ando@tier4.jp -localization/initial_pose_button_panel/** yamato.ando@tier4.jp -localization/localization_error_monitor/** yamato.ando@tier4.jp -localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/pose2twist/** yamato.ando@tier4.jp -localization/pose_initializer/** isamu.takagi@tier4.jp yamato.ando@tier4.jp -localization/stop_filter/** koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/twist2accel/** koji.minoda@tier4.jp yamato.ando@tier4.jp -localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp -localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp -localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp -localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp -map/map_height_fitter/** isamu.takagi@tier4.jp yamato.ando@tier4.jp -map/map_loader/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp +localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@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/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** masahiro.sakamoto@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/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@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 map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp @@ -116,29 +121,36 @@ perception/map_based_prediction/** shunsuke.miura@tier4.jp tomoya.kimura@tier4.j perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp +perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/probabilistic_occupancy_grid_map/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/radar_object_tracker/** satoshi.tanaka@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp +perception/tensorrt_classifier/** mingyu.li@tier4.jp perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp -perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp +perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/traffic_light_fine_detector/** mingyu.li@tier4.jp +perception/traffic_light_fine_detector/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp -perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp +perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +perception/traffic_light_occlusion_predictor/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@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 taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@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/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_intersection_module/** 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 +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 planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@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 yutaka.shimizu@tier4.jp planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @@ -146,6 +158,7 @@ planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakab planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @@ -156,10 +169,11 @@ planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@ planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@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 planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @@ -170,16 +184,16 @@ planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp -sensing/geo_pos_conv/** yamato.ando@tier4.jp sensing/gnss_poser/** yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp -sensing/imu_corrector/** yamato.ando@tier4.jp +sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp +sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @@ -201,7 +215,7 @@ system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp -tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai +tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml new file mode 100644 index 0000000000000..0ba732b580204 --- /dev/null +++ b/.github/workflows/openai-pr-reviewer.yaml @@ -0,0 +1,39 @@ +name: Code Review By ChatGPT + +permissions: + contents: read + pull-requests: write + +on: + pull_request_target: + types: + - labeled + pull_request_review_comment: + types: [created] + +concurrency: + group: ${{ github.repository }}-${{ github.event.number || github.head_ref || + github.sha }}-${{ github.workflow }}-${{ github.event_name == + 'pull_request_review_comment' && 'pr_comment' || 'pr' }} + cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} + +jobs: + prevent-no-label-execution: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: openai-pr-reviewer + review: + needs: prevent-no-label-execution + if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + runs-on: ubuntu-latest + steps: + - uses: fluxninja/openai-pr-reviewer@latest + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} + with: + debug: false + review_simple_changes: false + review_comment_lgtm: false + openai_light_model: gpt-3.5-turbo-0613 + openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp new file mode 100644 index 0000000000000..5711c9a86d12a --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/perception.hpp @@ -0,0 +1,36 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_ +#define AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_ + +#include + +#include + +namespace autoware_ad_api::perception +{ + +struct DynamicObjectArray +{ + using Message = autoware_adapi_v1_msgs::msg::DynamicObjectArray; + static constexpr char name[] = "/api/perception/objects"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::perception + +#endif // AUTOWARE_AD_API_SPECS__PERCEPTION_HPP_ diff --git a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp index ae142badec4eb..f050628f32f25 100644 --- a/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp +++ b/common/autoware_auto_geometry/include/geometry/bounding_box/eigenbox_2d.hpp @@ -98,7 +98,7 @@ Covariance2d covariance_2d(const IT begin, const IT end) /// \param[out] eig_vec1 First eigenvector /// \param[out] eig_vec2 Second eigenvector /// \tparam PointT Point type that has at least float members x and y -/// \return A pairt of eigenvalues: The first is the larger eigenvalue +/// \return A pair of eigenvalues: The first is the larger eigenvalue /// \throw std::runtime error if you would get degenerate covariance template std::pair eig_2d( 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 84fa359295613..5e42622b19ce9 100644 --- a/common/autoware_auto_geometry/test/include/test_bounding_box.hpp +++ b/common/autoware_auto_geometry/test/include/test_bounding_box.hpp @@ -336,7 +336,7 @@ TYPED_TEST(BoxTest, Line3) /_/ <-- first guess is suboptimal */ -TYPED_TEST(BoxTest, SuboptInit) +TYPED_TEST(BoxTest, SuboptimalInit) { this->points.insert( this->points.begin(), 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 8e2c20f10ac73..50e946c416270 100644 --- a/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp +++ b/common/autoware_auto_geometry/test/include/test_spatial_hash.hpp @@ -145,9 +145,9 @@ TYPED_TEST(TypedSpatialHashTest, Oob) // loop through all points float32_t r = dr + this->EPS; const uint32_t n_pts = PTS_PER_RING; - const auto & nbrs = hash.near(this->ref, r); + const auto & neighbors = hash.near(this->ref, r); uint32_t points_seen = 0U; - for (const auto itd : nbrs) { + for (const auto itd : neighbors) { const PointT & pt = itd; const float32_t dist = sqrtf((pt.x * pt.x) + (pt.y * pt.y)); ASSERT_LT(dist, r); 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 35ae240863b10..43e3a3ad08adf 100644 --- a/common/autoware_auto_geometry/test/src/test_convex_hull.cpp +++ b/common/autoware_auto_geometry/test/src/test_convex_hull.cpp @@ -139,7 +139,7 @@ TYPED_TEST(TypedConvexHullTest, Quadrilateral) } // test that things get reordered to ccw -TYPED_TEST(TypedConvexHullTest, Quadhull) +TYPED_TEST(TypedConvexHullTest, QuadHull) { std::vector data( {this->make(1, 1, 1), this->make(5, 1, 2), this->make(2, 6, 3), this->make(3, 3, 4), 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 7e8f510b1272e..2b79d4ff0f22b 100644 --- a/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp +++ b/common/autoware_auto_geometry/test/src/test_hull_pockets.cpp @@ -110,7 +110,7 @@ TYPED_TEST(TypedHullPocketsTest, Box) // | | // +------------------------------+ // This should come up with a single box on the top left. -TYPED_TEST(TypedHullPocketsTest, Ushape) +TYPED_TEST(TypedHullPocketsTest, UShape) { const auto polygon = std::vectormake(0, 0, 0))>{ this->make(0, 0, 0), this->make(5, 0, 0), this->make(5, 4.5, 0), this->make(4, 5, 0), 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/object_detection/object_polygon_display_base.hpp index 48f7d18ce67a5..b20a835f80e39 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -346,9 +346,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { std_msgs::msg::ColorRGBA sample_color; sample_color.r = 1.0; - sample_color.g = 0.0; - sample_color.b = 1.0; - colors.push_back(sample_color); // magenta + sample_color.g = 0.65; + sample_color.b = 0.0; + colors.push_back(sample_color); // orange + sample_color.r = 1.0; + sample_color.g = 1.0; + sample_color.b = 0.0; + colors.push_back(sample_color); // yellow sample_color.r = 0.69; sample_color.g = 1.0; sample_color.b = 0.18; @@ -361,22 +365,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase sample_color.g = 1.0; sample_color.b = 0.0; colors.push_back(sample_color); // chartreuse green - sample_color.r = 0.12; - sample_color.g = 0.56; - sample_color.b = 1.0; - colors.push_back(sample_color); // dodger blue sample_color.r = 0.0; sample_color.g = 1.0; sample_color.b = 1.0; colors.push_back(sample_color); // cyan - sample_color.r = 0.54; - sample_color.g = 0.168; - sample_color.b = 0.886; - colors.push_back(sample_color); // blueviolet - sample_color.r = 0.0; - sample_color.g = 1.0; - sample_color.b = 0.5; - colors.push_back(sample_color); // spring green + sample_color.r = 0.53; + sample_color.g = 0.81; + sample_color.b = 0.98; + colors.push_back(sample_color); // light skyblue + sample_color.r = 1.0; + sample_color.g = 0.41; + sample_color.b = 0.71; + colors.push_back(sample_color); // hot pink } double get_line_width() { return m_line_width_property.getFloat(); } 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 73a524e130776..894e377a6f851 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 @@ -64,8 +64,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( marker_ptr->color = path_confidence_color; marker_ptr->pose.position = predicted_path.path.back().position; marker_ptr->text = std::to_string(predicted_path.confidence); - marker_ptr->color.a = std::max( - static_cast(std::min(static_cast(predicted_path.confidence), 0.999)), 0.5); + marker_ptr->color.a = 0.5; return marker_ptr; } @@ -81,9 +80,8 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); marker_ptr->pose = initPose(); marker_ptr->color = predicted_path_color; - marker_ptr->color.a = std::max( - static_cast(std::min(static_cast(predicted_path.confidence), 0.999)), 0.5); - marker_ptr->scale.x = 0.03 * marker_ptr->color.a; + marker_ptr->color.a = 0.6; + marker_ptr->scale.x = 0.015; calc_path_line_list(predicted_path, marker_ptr->points, is_simple); for (size_t k = 0; k < marker_ptr->points.size(); ++k) { marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0; diff --git a/common/component_interface_specs/include/component_interface_specs/map.hpp b/common/component_interface_specs/include/component_interface_specs/map.hpp index f0cce7a7f97a2..dc162d4a7267a 100644 --- a/common/component_interface_specs/include/component_interface_specs/map.hpp +++ b/common/component_interface_specs/include/component_interface_specs/map.hpp @@ -25,7 +25,7 @@ namespace map_interface struct MapProjectorInfo { using Message = tier4_map_msgs::msg::MapProjectorInfo; - static constexpr char name[] = "/map/map_projector_type"; + static constexpr char name[] = "/map/map_projector_info"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp new file mode 100644 index 0000000000000..8665b9c35157d --- /dev/null +++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp @@ -0,0 +1,36 @@ +// 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 COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ +#define COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ + +#include + +#include + +namespace perception_interface +{ + +struct ObjectRecognition +{ + using Message = autoware_auto_perception_msgs::msg::PredictedObjects; + static constexpr char name[] = "/perception/object_recognition/objects"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace perception_interface + +#endif // COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ diff --git a/common/glog_component/CMakeLists.txt b/common/glog_component/CMakeLists.txt new file mode 100644 index 0000000000000..c2a68d0130b37 --- /dev/null +++ b/common/glog_component/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(glog_component) + +find_package(autoware_cmake REQUIRED) +autoware_package() + + +ament_auto_add_library(glog_component SHARED + src/glog_component.cpp +) +target_link_libraries(glog_component glog) + +rclcpp_components_register_node(glog_component + PLUGIN "GlogComponent" + EXECUTABLE glog_component_node +) + +ament_auto_package() diff --git a/common/glog_component/README.md b/common/glog_component/README.md new file mode 100644 index 0000000000000..94a40fe32e40d --- /dev/null +++ b/common/glog_component/README.md @@ -0,0 +1,29 @@ +# glog_component + +This package provides the glog (google logging library) feature as a ros2 component library. This is used to dynamically load the glog feature with container. + +See the [glog github](https://github.com/google/glog) for the details of its features. + +## Example + +When you load the `glog_component` in container, the launch file can be like below: + +```py +glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", +) + +container = ComposableNodeContainer( + name="my_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + component1, + component2, + glog_component, + ], +) +``` diff --git a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp b/common/glog_component/include/glog_component/glog_component.hpp similarity index 64% rename from planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp rename to common/glog_component/include/glog_component/glog_component.hpp index f6a7a10b21db1..d940363d75ac4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/module_status.hpp +++ b/common/glog_component/include/glog_component/glog_component.hpp @@ -12,18 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ -#define BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ +#ifndef GLOG_COMPONENT__GLOG_COMPONENT_HPP_ +#define GLOG_COMPONENT__GLOG_COMPONENT_HPP_ -namespace behavior_path_planner +#include + +#include + +class GlogComponent : public rclcpp::Node { -enum class ModuleStatus { - IDLE = 0, - RUNNING = 1, - SUCCESS = 2, - FAILURE = 3, - // SKIPPED = 4, +public: + explicit GlogComponent(const rclcpp::NodeOptions & node_options); }; -} // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__MODULE_STATUS_HPP_ +#endif // GLOG_COMPONENT__GLOG_COMPONENT_HPP_ diff --git a/sensing/geo_pos_conv/package.xml b/common/glog_component/package.xml similarity index 52% rename from sensing/geo_pos_conv/package.xml rename to common/glog_component/package.xml index b82ea443ef7d5..0d6e7daac1de3 100644 --- a/sensing/geo_pos_conv/package.xml +++ b/common/glog_component/package.xml @@ -1,20 +1,21 @@ - geo_pos_conv - 2.0.0 - The ROS 2 geo_pos_conv package - Yamato Ando - + glog_component + 0.1.0 + The glog_component package + Takamasa Horibe Apache License 2.0 + Takamasa Horibe + + ament_cmake ament_cmake_auto autoware_cmake + libgoogle-glog-dev rclcpp - - ament_lint_auto - autoware_lint_common + rclcpp_components ament_cmake diff --git a/common/glog_component/src/glog_component.cpp b/common/glog_component/src/glog_component.cpp new file mode 100644 index 0000000000000..9e7e70da6c884 --- /dev/null +++ b/common/glog_component/src/glog_component.cpp @@ -0,0 +1,25 @@ +// 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 "glog_component/glog_component.hpp" + +GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options) +: Node("glog_component", node_options) +{ + google::InitGoogleLogging("glog_component"); + google::InstallFailureSignalHandler(); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(GlogComponent) diff --git a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp index 545bdac7b630d..5c1c98488b0f6 100644 --- a/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp +++ b/common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp @@ -75,7 +75,7 @@ void AccelBrakeMapCalibratorButtonPanel::onInitialize() &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1)); client_ = raw_node->create_client( - "/accel_brake_map_calibrator/update_map_dir"); + "/vehicle/calibration/accel_brake_map_calibrator/update_map_dir"); } void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest( @@ -118,25 +118,28 @@ void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton() status_label_->setText("executing calibration..."); std::thread thread([this] { - auto req = std::make_shared(); - req->path = ""; - - client_->async_send_request( - req, - [this]( - [[maybe_unused]] rclcpp::Client::SharedFuture - result) { - status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); - status_label_->setText("OK!!!"); - - // wait 3 second - after_calib_ = true; - rclcpp::Rate(3.0).sleep(); - after_calib_ = false; - - // unlock button - calibration_button_->setEnabled(true); - }); + if (!client_->wait_for_service(std::chrono::seconds(1))) { + status_label_->setStyleSheet("QLabel { background-color : red;}"); + status_label_->setText("service server not found"); + + } else { + auto req = std::make_shared(); + req->path = ""; + client_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client< + tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>::SharedFuture result) {}); + + status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); + status_label_->setText("OK!!!"); + } + + // wait 3 second + after_calib_ = true; + rclcpp::Rate(1.0 / 3.0).sleep(); + after_calib_ = false; + + // unlock button + calibration_button_->setEnabled(true); }); thread.detach(); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 8f4e56c84164e..aa1dcfcd1651d 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -166,6 +166,10 @@ QGroupBox * AutowareStatePanel::makeLocalizationGroup() localization_label_ptr_->setStyleSheet("border:1px solid black;"); grid->addWidget(localization_label_ptr_, 0, 0); + init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + grid->addWidget(init_by_gnss_button_ptr_, 1, 0); + group->setLayout(grid); return group; } @@ -249,6 +253,9 @@ void AutowareStatePanel::onInitialize() "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onLocalization, this, _1)); + client_init_by_gnss_ = + raw_node_->create_client("/api/localization/initialize"); + // Motion sub_motion_ = raw_node_->create_subscription( "/api/motion/state", rclcpp::QoS{1}.transient_local(), @@ -587,6 +594,11 @@ void AutowareStatePanel::onClickClearRoute() callServiceWithoutResponse(client_clear_route_); } +void AutowareStatePanel::onClickInitByGnss() +{ + callServiceWithoutResponse(client_init_by_gnss_); +} + void AutowareStatePanel::onClickAcceptStart() { callServiceWithoutResponse(client_accept_start_); 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 3a9bd6a542401..eba3e4eacd275 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ class AutowareStatePanel : public rviz_common::Panel using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; using MRMState = autoware_adapi_v1_msgs::msg::MrmState; @@ -69,6 +71,7 @@ public Q_SLOTS: // NOLINT for Qt void onClickAutowareControl(); void onClickDirectControl(); void onClickClearRoute(); + void onClickInitByGnss(); void onClickAcceptStart(); void onClickVelocityLimit(); void onClickEmergencyButton(); @@ -130,7 +133,10 @@ public Q_SLOTS: // NOLINT for Qt // Localization QLabel * localization_label_ptr_{nullptr}; + QPushButton * init_by_gnss_button_ptr_{nullptr}; + rclcpp::Subscription::SharedPtr sub_localization_; + rclcpp::Client::SharedPtr client_init_by_gnss_; void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); diff --git a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt index 184fe34fd8e96..de535d3dace05 100644 --- a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt +++ b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt @@ -17,6 +17,7 @@ set(HEADERS src/tools/steering_angle.hpp src/tools/jsk_overlay_utils.hpp src/tools/velocity_history.hpp + src/tools/acceleration_meter.hpp ) ## Declare a C++ library @@ -26,6 +27,7 @@ ament_auto_add_library(tier4_vehicle_rviz_plugin SHARED src/tools/steering_angle.cpp src/tools/jsk_overlay_utils.cpp src/tools/velocity_history.cpp + src/tools/acceleration_meter.cpp ${HEADERS} ) diff --git a/common/tier4_vehicle_rviz_plugin/README.md b/common/tier4_vehicle_rviz_plugin/README.md index ffbefeb1d7652..09576ac327bec 100644 --- a/common/tier4_vehicle_rviz_plugin/README.md +++ b/common/tier4_vehicle_rviz_plugin/README.md @@ -5,7 +5,7 @@ Note that jsk_overlay_utils.cpp and jsk_overlay_utils.hpp are BSD license. ## Purpose -This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal and steering status. +This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal, steering status and acceleration. ## Inputs / Outputs @@ -16,6 +16,7 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | | `/control/turn_signal_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | | `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | ## Parameter @@ -64,6 +65,20 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | `property_velocity_color_` | QColor | Qt::black | Color of velocity history | | `property_vel_max_` | float | 3.0 | Color Border Vel Max [m/s] | +#### AccelerationMeter + +| Name | Type | Default Value | Description | +| ----------------------------------- | ------ | -------------------- | ------------------------------------------------ | +| `property_normal_text_color_` | QColor | QColor(25, 255, 240) | Normal text color | +| `property_emergency_text_color_` | QColor | QColor(255, 80, 80) | Emergency acceleration color | +| `property_left_` | int | 896 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_length_` | int | 256 | Height of the plotter window [px] | +| `property_value_height_offset_` | int | 0 | Height offset of the plotter window [px] | +| `property_value_scale_` | float | 1 / 6.667 | Value text scale | +| `property_emergency_threshold_max_` | float | 1.0 | Max acceleration threshold for emergency [m/s^2] | +| `property_emergency_threshold_min_` | float | -2.5 | Min acceleration threshold for emergency [m/s^2] | + ## Assumptions / Known limits TBD. diff --git a/common/tier4_vehicle_rviz_plugin/icons/classes/AccelerationMeter.png b/common/tier4_vehicle_rviz_plugin/icons/classes/AccelerationMeter.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_vehicle_rviz_plugin/icons/classes/AccelerationMeter.png differ diff --git a/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml b/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml index 35431a17d5937..65f7414831f8f 100644 --- a/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_vehicle_rviz_plugin/plugins/plugin_description.xml @@ -15,4 +15,8 @@ type="rviz_plugins::VelocityHistoryDisplay" base_class_type="rviz_common::Display"> + + diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp new file mode 100644 index 0000000000000..3b3810a782a91 --- /dev/null +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.cpp @@ -0,0 +1,247 @@ +// 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 "acceleration_meter.hpp" + +#include +#include + +#include + +#include +namespace rviz_plugins +{ +AccelerationMeterDisplay::AccelerationMeterDisplay() +{ + 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(896 * scale)); + const auto top = static_cast(std::round(128 * scale)); + const auto length = static_cast(std::round(256 * scale)); + + property_normal_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_emergency_text_color_ = new rviz_common::properties::ColorProperty( + "Emergency Color", QColor(255, 80, 80), "emergency 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_length_ = new rviz_common::properties::IntProperty( + "Length", length, "Length of the plotter window", this, SLOT(updateVisualization()), this); + property_length_->setMin(10); + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_emergency_threshold_max_ = new rviz_common::properties::FloatProperty( + "Max Emergency Threshold", 1.0, "Max emergency threshold for acceleration", this, + SLOT(updateVisualization()), this); + property_emergency_threshold_max_->setMin(0.01); + property_emergency_threshold_min_ = new rviz_common::properties::FloatProperty( + "Min Emergency Threshold", -2.5, "Min emergency threshold for acceleration", this, + SLOT(updateVisualization()), this); + property_emergency_threshold_min_->setMax(-0.01); + property_value_scale_ = new rviz_common::properties::FloatProperty( + "Value Scale", 1.0 / 6.667, "Value scale", this, SLOT(updateVisualization()), this); + property_value_scale_->setMin(0.01); +} + +AccelerationMeterDisplay::~AccelerationMeterDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void AccelerationMeterDisplay::onInitialize() +{ + RTDClass::onInitialize(); + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "AccelerationMeterDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void AccelerationMeterDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void AccelerationMeterDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void AccelerationMeterDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + double acceleration_x = 0; + { + std::lock_guard message_lock(mutex_); + if (last_msg_ptr_) { + acceleration_x = last_msg_ptr_->accel.accel.linear.x; + } + } + + 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_; + + // meter + QColor white_color(Qt::white); + white_color.setAlpha(255); + const float acceleration_ratio = std::min( + std::max(acceleration_x - meter_min_acceleration_, 0.0) / + (meter_max_acceleration_ - meter_min_acceleration_), + 1.0); + const float theta = + (acceleration_ratio * (meter_max_angle_ - meter_min_angle_)) + meter_min_angle_ + M_PI_2; + + painter.setPen(QPen(white_color, hand_width_, Qt::SolidLine)); + painter.drawLine( + w * 0.5, h * 0.5, + (w * 0.5) + + (static_cast(w) * 0.5 - (static_cast(hand_width_) * 0.5)) * std::cos(theta), + (h * 0.5) + + (static_cast(h) * 0.5 - (static_cast(hand_width_) * 0.5)) * std::sin(theta)); + + painter.setPen(QPen(white_color, line_width_, Qt::SolidLine)); + painter.drawLine(min_range_line_.x0, min_range_line_.y0, min_range_line_.x1, min_range_line_.y1); + painter.drawLine(max_range_line_.x0, max_range_line_.y0, max_range_line_.x1, max_range_line_.y1); + painter.drawArc( + outer_arc_.x0, outer_arc_.y0, outer_arc_.x1, outer_arc_.y1, outer_arc_.start_angle * 16, + outer_arc_.end_angle * 16); + painter.drawArc( + inner_arc_.x0, inner_arc_.y0, inner_arc_.x1, inner_arc_.y1, inner_arc_.start_angle * 16, + inner_arc_.end_angle * 16); + + // text + QColor text_color; + if ( + (acceleration_x > property_emergency_threshold_max_->getFloat()) || + (acceleration_x < + property_emergency_threshold_min_ + ->getFloat())) { // Write in Red if acceleration is over emergency threshold. + text_color = property_emergency_text_color_->getColor(); + } else { + text_color = property_normal_text_color_->getColor(); + } + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize( + std::max(static_cast(static_cast(w) * property_value_scale_->getFloat()), 1)); + font.setBold(true); + painter.setFont(font); + std::ostringstream acceleration_ss; + // Write acceleration in m/s^2 for debugging usage. + acceleration_ss << std::fixed << std::setprecision(2) << acceleration_x << "m/s^2"; + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter, + acceleration_ss.str().c_str()); + + painter.end(); + updateVisualization(); +} + +void AccelerationMeterDisplay::processMessage( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void AccelerationMeterDisplay::updateVisualization() +{ + // Transferred from ConsoleMeter for unified design + overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + const float min_range_theta = meter_max_angle_ + M_PI_2; + const float max_range_theta = meter_min_angle_ + M_PI_2; + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + min_range_line_.x0 = + (w / 2.0) + line_width_ / 2.0 + (static_cast(w) / 8.0) * std::cos(min_range_theta); + min_range_line_.y0 = + (h / 2.0) + line_width_ / 2.0 + (static_cast(h) / 8.0) * std::sin(min_range_theta); + min_range_line_.x1 = + (w / 2.0) + line_width_ / 2.0 + + (static_cast(w) / 2.0 - (line_width_ / 2.0)) * std::cos(min_range_theta); + min_range_line_.y1 = + (h / 2.0) + line_width_ / 2.0 + + (static_cast(h) / 2.0 - (line_width_ / 2.0)) * std::sin(min_range_theta); + max_range_line_.x0 = + (w / 2.0) + line_width_ / 2.0 + (static_cast(w) / 8.0) * std::cos(max_range_theta); + max_range_line_.y0 = + (h / 2.0) + line_width_ / 2.0 + (static_cast(h) / 8.0) * std::sin(max_range_theta); + max_range_line_.x1 = + (w * 0.5) + line_width_ * 0.5 + + (static_cast(w) / 2.0 - (line_width_ * 0.5)) * std::cos(max_range_theta); + max_range_line_.y1 = + (h * 0.5) + line_width_ * 0.5 + + (static_cast(h) / 2.0 - (line_width_ * 0.5)) * std::sin(max_range_theta); + inner_arc_.x0 = line_width_ / 2.0; + inner_arc_.y0 = line_width_ / 2.0; + inner_arc_.x1 = w; + inner_arc_.y1 = h; + inner_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); + inner_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); + outer_arc_.x0 = w * 3 / 8; + outer_arc_.y0 = h * 3 / 8; + outer_arc_.x1 = w / 4; + outer_arc_.y1 = h / 4; + outer_arc_.start_angle = tier4_autoware_utils::rad2deg(min_range_theta - M_PI); + outer_arc_.end_angle = tier4_autoware_utils::rad2deg(max_range_theta - min_range_theta); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AccelerationMeterDisplay, rviz_common::Display) diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp new file mode 100644 index 0000000000000..371480eccdbce --- /dev/null +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -0,0 +1,96 @@ +// 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 TOOLS__ACCELERATION_METER_HPP_ +#define TOOLS__ACCELERATION_METER_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include +#include + +#include +#endif + +namespace rviz_plugins +{ +class AccelerationMeterDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + AccelerationMeterDisplay(); + ~AccelerationMeterDisplay() override; + + void onInitialize() override; + void onDisable() override; + void onEnable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_normal_text_color_; + rviz_common::properties::ColorProperty * property_emergency_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_length_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::FloatProperty * property_emergency_threshold_max_; + rviz_common::properties::FloatProperty * property_emergency_threshold_min_; + // QImage hud_; + +private: + static constexpr float meter_min_acceleration_ = -10.0f; + static constexpr float meter_max_acceleration_ = 10.0f; + static constexpr float meter_min_angle_ = tier4_autoware_utils::deg2rad(40.f); + static constexpr float meter_max_angle_ = tier4_autoware_utils::deg2rad(320.f); + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + struct Line // for drawLine + { + int x0, y0; + int x1, y1; + }; + Line min_range_line_; + Line max_range_line_; + struct Arc // for drawArc + { + int x0, y0; + int x1, y1; + float start_angle, end_angle; + }; + Arc inner_arc_; + Arc outer_arc_; + + std::mutex mutex_; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // TOOLS__ACCELERATION_METER_HPP_ diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index f13e971b4d6d6..5d3684d0351c6 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -150,7 +150,11 @@ void ConsoleMeterDisplay::update(float wall_dt, float ros_dt) font.setBold(true); painter.setFont(font); std::ostringstream velocity_ss; - velocity_ss << std::fixed << std::setprecision(2) << linear_x * 3.6 << "km/h"; + if (last_msg_ptr_) { + velocity_ss << std::fixed << std::setprecision(2) << linear_x * 3.6 << "km/h"; + } else { + velocity_ss << "Not received"; + } painter.drawText( 0, std::min(property_value_height_offset_->getInt(), h - 1), w, std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter, diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp index 18977367afbce..60a81e45c9c29 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -153,7 +153,11 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt) font.setBold(true); painter.setFont(font); std::ostringstream steering_angle_ss; - steering_angle_ss << std::fixed << std::setprecision(1) << steering * 180.0 / M_PI << "deg"; + if (last_msg_ptr_) { + steering_angle_ss << std::fixed << std::setprecision(1) << steering * 180.0 / M_PI << "deg"; + } else { + steering_angle_ss << "Not received"; + } painter.drawText( 0, std::min(property_value_height_offset_->getInt(), h - 1), w, std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter, diff --git a/common/traffic_light_utils/CMakeLists.txt b/common/traffic_light_utils/CMakeLists.txt index 567e7653b524f..741bdd585ed7f 100644 --- a/common/traffic_light_utils/CMakeLists.txt +++ b/common/traffic_light_utils/CMakeLists.txt @@ -5,9 +5,18 @@ find_package(autoware_cmake REQUIRED) autoware_package() find_package(Boost REQUIRED) +find_package(autoware_cmake REQUIRED) ament_auto_add_library(traffic_light_utils SHARED src/traffic_light_utils.cpp ) +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_traffic_light_utils ${TEST_SOURCES}) + target_include_directories(test_traffic_light_utils PRIVATE src/include) + target_link_libraries(test_traffic_light_utils traffic_light_utils) +endif() + ament_auto_package() diff --git a/common/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index 48c6212a5909e..de05355eafd66 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -11,6 +11,10 @@ ament_cmake_auto autoware_cmake + ament_cmake_ros + ament_lint_auto + autoware_lint_common + lanelet2_extension tier4_perception_msgs diff --git a/common/traffic_light_utils/test/test_traffic_light_utils.cpp b/common/traffic_light_utils/test/test_traffic_light_utils.cpp new file mode 100644 index 0000000000000..bd777265b4195 --- /dev/null +++ b/common/traffic_light_utils/test/test_traffic_light_utils.cpp @@ -0,0 +1,96 @@ +// 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 "gtest/gtest.h" +#include "traffic_light_utils/traffic_light_utils.hpp" + +namespace traffic_light_utils +{ + +TEST(isRoiValid, roi_validity) +{ + tier4_perception_msgs::msg::TrafficLightRoi test_roi; + test_roi.roi.x_offset = 300; + test_roi.roi.y_offset = 200; + test_roi.roi.width = 340; + test_roi.roi.height = 200; + uint32_t img_width = 640; + uint32_t img_heigh = 480; + EXPECT_FALSE(isRoiValid(test_roi, img_width, img_heigh)); + test_roi.roi.width = 339; + EXPECT_TRUE(isRoiValid(test_roi, img_width, img_heigh)); +} + +TEST(setRoiInvalid, set_roi_size) +{ + tier4_perception_msgs::msg::TrafficLightRoi test_roi; + test_roi.roi.x_offset = 300; + test_roi.roi.y_offset = 200; + test_roi.roi.width = 300; + test_roi.roi.height = 200; + EXPECT_EQ(test_roi.roi.width, (uint32_t)300); + EXPECT_EQ(test_roi.roi.height, (uint32_t)200); + setRoiInvalid(test_roi); + EXPECT_EQ(test_roi.roi.width, (uint32_t)0); + EXPECT_EQ(test_roi.roi.height, (uint32_t)0); +} + +TEST(isSignalUnknown, signal_element) +{ + tier4_perception_msgs::msg::TrafficSignal test_signal; + tier4_perception_msgs::msg::TrafficLightElement element; + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + test_signal.elements.push_back(element); + EXPECT_TRUE(isSignalUnknown(test_signal)); + test_signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::RED; + EXPECT_FALSE(isSignalUnknown(test_signal)); +} + +TEST(setSignalUnknown, set_signal_element) +{ + tier4_perception_msgs::msg::TrafficSignal test_signal; + tier4_perception_msgs::msg::TrafficLightElement element; + element.color = tier4_perception_msgs::msg::TrafficLightElement::RED; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::CROSS; + test_signal.elements.push_back(element); + EXPECT_EQ(test_signal.elements[0].color, tier4_perception_msgs::msg::TrafficLightElement::RED); + EXPECT_EQ(test_signal.elements[0].shape, tier4_perception_msgs::msg::TrafficLightElement::CROSS); + setSignalUnknown(test_signal, 1.23f); + EXPECT_EQ( + test_signal.elements[0].color, tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN); + EXPECT_EQ( + test_signal.elements[0].shape, tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN); + EXPECT_FLOAT_EQ(test_signal.elements[0].confidence, (float)1.23); +} + +TEST(getTrafficLightCenter, get_signal) +{ + lanelet::LineString3d lineString; + lanelet::Point3d p0(0, 0, 0, 0); + lanelet::Point3d p1(1, 1, 1, 1); + lanelet::Point3d p2(2, 2, 2, 2); + lanelet::Point3d p3(3, 3, 3, 3); + lineString.push_back(p0); + lineString.push_back(p1); + lineString.push_back(p2); + lineString.push_back(p3); + + lanelet::ConstLineString3d test_light(lineString); + EXPECT_FLOAT_EQ(getTrafficLightCenter(test_light).x(), (float)1.5); + EXPECT_FLOAT_EQ(getTrafficLightCenter(test_light).y(), (float)1.5); + EXPECT_FLOAT_EQ(getTrafficLightCenter(test_light).z(), (float)1.5); +} + +} // namespace traffic_light_utils diff --git a/control/control_validator/CMakeLists.txt b/control/control_validator/CMakeLists.txt new file mode 100644 index 0000000000000..fab942c4dc001 --- /dev/null +++ b/control/control_validator/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(control_validator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(control_validator_helpers SHARED + src/utils.cpp + src/debug_marker.cpp +) + +# control validator +ament_auto_add_library(control_validator_component SHARED + include/control_validator/control_validator.hpp + src/control_validator.cpp +) +target_link_libraries(control_validator_component control_validator_helpers) +rclcpp_components_register_node(control_validator_component + PLUGIN "control_validator::ControlValidator" + EXECUTABLE control_validator_node +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/ControlValidatorStatus.msg" + DEPENDENCIES builtin_interfaces +) + +# to use a message defined in the same package +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(control_validator_component + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(control_validator_component "${cpp_typesupport_target}") +endif() + +# if(BUILD_TESTING) +# endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/control/control_validator/README.md b/control/control_validator/README.md new file mode 100644 index 0000000000000..3d78721a0a040 --- /dev/null +++ b/control/control_validator/README.md @@ -0,0 +1,58 @@ +# Control Validator + +The `control_validator` is a module that checks the validity of the output of the control component. The status of the validation can be viewed in the `/diagnostics` topic. + +![control_validator](./image/control_validator.drawio.svg) + +## Supported features + +The following features are supported for the validation and can have thresholds set by parameters: + +- **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold. + +![trajectory_deviation](./image/trajectory_deviation.drawio.svg) + +Other features are to be implemented. + +## Inputs/Outputs + +### Inputs + +The `control_validator` takes in the following inputs: + +| Name | Type | Description | +| ------------------------------ | ------------------------------------- | ------------------------------------------------------------------------------ | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/reference_trajectory` | autoware_auto_control_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | +| `~/input/predicted_trajectory` | autoware_auto_control_msgs/Trajectory | predicted trajectory which is outputted from control module | + +### Outputs + +It outputs the following: + +| Name | Type | Description | +| ---------------------------- | ---------------------------------------- | ------------------------------------------------------------------------- | +| `~/output/validation_status` | control_validator/ControlValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid | +| `/diagnostics` | diagnostic_msgs/DiagnosticStatus | diagnostics to report errors | + +## Parameters + +The following parameters can be set for the `control_validator`: + +### System parameters + +| Name | Type | Description | Default value | +| :--------------------------- | :--- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `publish_diag` | bool | if true, diagnostics msg is published. | true | +| `diag_error_count_threshold` | int | the Diag will be set to ERROR when the number of consecutive invalid trajectory exceeds this threshold. (For example, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | true | +| `display_on_terminal` | bool | show error msg on terminal | true | + +### Algorithm parameters + +#### Thresholds + +The input trajectory is detected as invalid if the index exceeds the following thresholds. + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0 | diff --git a/control/control_validator/config/control_validator.param.yaml b/control/control_validator/config/control_validator.param.yaml new file mode 100644 index 0000000000000..7bbe6a466799b --- /dev/null +++ b/control/control_validator/config/control_validator.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + + publish_diag: true # if true, diagnostic msg is published + + # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR. + # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if + # the next trajectory is valid.) + diag_error_count_threshold: 0 + + display_on_terminal: true # show error msg on terminal + + thresholds: + max_distance_deviation: 1.0 diff --git a/control/control_validator/image/control_validator.drawio.svg b/control/control_validator/image/control_validator.drawio.svg new file mode 100644 index 0000000000000..1b226cbd3ec85 --- /dev/null +++ b/control/control_validator/image/control_validator.drawio.svg @@ -0,0 +1,155 @@ + + + + + + +
+
control_validator
+
+ + + + + + +
+
Predicted trajectory
+
+ + + + + + +
+
ego_kinematics
+
+ + + + + + +
+
DiagnosticStatus
+
+ + + + + + +
+
ControlValidatorStatus
+
+ + + + + +
+
+ + Predicted trajectory to +
+ be validated + +
+
+ + + + + +
+
+ Used for the validation +
+
+ + + + + +
+
+ To send validation result to the system: OK/ERROR +
+
+ + + + + +
+
+ To show the result and the reason for other modules/users +
+
+ + + + + + +
+
Planning trajectory
+
+ + + + + +
+
+ + Reference trajectory to be followed + +
+
+ + + + diff --git a/control/control_validator/image/trajectory_deviation.drawio.svg b/control/control_validator/image/trajectory_deviation.drawio.svg new file mode 100644 index 0000000000000..cdad355c5bb1e --- /dev/null +++ b/control/control_validator/image/trajectory_deviation.drawio.svg @@ -0,0 +1,112 @@ + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Reference Trajectory +
+
+
+ + Reference Trajectory + + + + + + +
+
+
+ Predicted Trajectory +
+
+
+ + Predicted Trajectory + + + + + + + + + +
+
+
+ max distance deviation +
+
+
+ + max distance deviation + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp new file mode 100644 index 0000000000000..48b7eba7412a2 --- /dev/null +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -0,0 +1,100 @@ +// 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 CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#define CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ + +#include "control_validator/debug_marker.hpp" +#include "control_validator/msg/control_validator_status.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include + +#include +#include +#include + +#include +#include + +namespace control_validator +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using control_validator::msg::ControlValidatorStatus; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using nav_msgs::msg::Odometry; + +struct ValidationParams +{ + double max_distance_deviation_threshold; +}; + +class ControlValidator : public rclcpp::Node +{ +public: + explicit ControlValidator(const rclcpp::NodeOptions & options); + + void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); + void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); + + bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); + +private: + void setupDiag(); + + void setupParameters(); + + bool isDataReady(); + + void validate(const Trajectory & trajectory); + + void publishPredictedTrajectory(); + void publishDebugInfo(); + void displayStatus(); + + void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); + + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_reference_traj_; + rclcpp::Subscription::SharedPtr sub_predicted_traj_; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_markers_; + + // system parameters + bool publish_diag_ = true; + int diag_error_count_threshold_ = 0; + bool display_on_terminal_ = true; + + Updater diag_updater_{this}; + + ControlValidatorStatus validation_status_; + ValidationParams validation_params_; // for thresholds + + vehicle_info_util::VehicleInfo vehicle_info_; + + bool isAllValid(const ControlValidatorStatus & status); + + Trajectory::ConstSharedPtr current_reference_trajectory_; + Trajectory::ConstSharedPtr current_predicted_trajectory_; + + Odometry::ConstSharedPtr current_kinematics_; + + std::shared_ptr debug_pose_publisher_; +}; +} // namespace control_validator + +#endif // CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ diff --git a/control/control_validator/include/control_validator/debug_marker.hpp b/control/control_validator/include/control_validator/debug_marker.hpp new file mode 100644 index 0000000000000..794912e085949 --- /dev/null +++ b/control/control_validator/include/control_validator/debug_marker.hpp @@ -0,0 +1,61 @@ +// 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 CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ +#define CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +class ControlValidatorDebugMarkerPublisher +{ +public: + explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); + + void pushPoseMarker( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, + int id = 0); + void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); + void pushVirtualWall(const geometry_msgs::msg::Pose & pose); + void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); + void publish(); + + void clearMarkers(); + +private: + rclcpp::Node * node_; + visualization_msgs::msg::MarkerArray marker_array_; + visualization_msgs::msg::MarkerArray marker_array_virtual_wall_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + std::map marker_id_; + + int getMarkerId(const std::string & ns) + { + if (marker_id_.count(ns) == 0) { + marker_id_[ns] = 0.0; + } + return marker_id_[ns]++; + } +}; + +#endif // CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp new file mode 100644 index 0000000000000..39ec316dbe437 --- /dev/null +++ b/control/control_validator/include/control_validator/utils.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 CONTROL_VALIDATOR__UTILS_HPP_ +#define CONTROL_VALIDATOR__UTILS_HPP_ + +#include +#include + +#include + +#include +#include +#include + +namespace control_validator +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using motion_utils::convertToTrajectory; +using motion_utils::convertToTrajectoryPointArray; +using TrajectoryPoints = std::vector; + +void shiftPose(Pose & pose, double longitudinal); + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory); + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory); + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points); + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); + +double calcMaxLateralDistance( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); +} // namespace control_validator + +#endif // CONTROL_VALIDATOR__UTILS_HPP_ diff --git a/control/control_validator/launch/control_validator.launch.xml b/control/control_validator/launch/control_validator.launch.xml new file mode 100644 index 0000000000000..9727e06e60523 --- /dev/null +++ b/control/control_validator/launch/control_validator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/control/control_validator/msg/ControlValidatorStatus.msg b/control/control_validator/msg/ControlValidatorStatus.msg new file mode 100644 index 0000000000000..242bede4ece89 --- /dev/null +++ b/control/control_validator/msg/ControlValidatorStatus.msg @@ -0,0 +1,9 @@ +builtin_interfaces/Time stamp + +# states +bool is_valid_max_distance_deviation + +# values +float64 max_distance_deviation + +int64 invalid_count diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml new file mode 100644 index 0000000000000..c46c2d237b605 --- /dev/null +++ b/control/control_validator/package.xml @@ -0,0 +1,41 @@ + + + + control_validator + 0.1.0 + ros node for control_validator + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + Apache License 2.0 + + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + + ament_cmake_auto + autoware_cmake + rosidl_default_generators + + autoware_auto_planning_msgs + diagnostic_updater + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp new file mode 100644 index 0000000000000..59b798a786722 --- /dev/null +++ b/control/control_validator/src/control_validator.cpp @@ -0,0 +1,213 @@ +// 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 "control_validator/control_validator.hpp" + +#include "control_validator/utils.hpp" + +#include +#include + +#include +#include +#include + +namespace control_validator +{ +using diagnostic_msgs::msg::DiagnosticStatus; + +ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) +: Node("control_validator", options) +{ + using std::placeholders::_1; + + sub_kinematics_ = create_subscription( + "~/input/kinematics", 1, + [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); + sub_reference_traj_ = create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&ControlValidator::onReferenceTrajectory, this, _1)); + sub_predicted_traj_ = create_subscription( + "~/input/predicted_trajectory", 1, + std::bind(&ControlValidator::onPredictedTrajectory, this, _1)); + + pub_status_ = create_publisher("~/output/validation_status", 1); + pub_markers_ = create_publisher("~/output/markers", 1); + + debug_pose_publisher_ = std::make_shared(this); + + setupParameters(); + + if (publish_diag_) { + setupDiag(); + } +} + +void ControlValidator::setupParameters() +{ + publish_diag_ = declare_parameter("publish_diag"); + diag_error_count_threshold_ = declare_parameter("diag_error_count_threshold"); + display_on_terminal_ = declare_parameter("display_on_terminal"); + + { + auto & p = validation_params_; + const std::string t = "thresholds."; + p.max_distance_deviation_threshold = declare_parameter(t + "max_distance_deviation"); + } + + try { + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + } catch (...) { + RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); + vehicle_info_.front_overhang_m = 0.5; + vehicle_info_.wheel_base_m = 4.0; + } +} + +void ControlValidator::setStatus( + DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) +{ + if (is_ok) { + stat.summary(DiagnosticStatus::OK, "validated."); + } else if (validation_status_.invalid_count < diag_error_count_threshold_) { + const auto warn_msg = msg + " (invalid count is less than error threshold: " + + std::to_string(validation_status_.invalid_count) + " < " + + std::to_string(diag_error_count_threshold_) + ")"; + stat.summary(DiagnosticStatus::WARN, warn_msg); + } else { + stat.summary(DiagnosticStatus::ERROR, msg); + } +} + +void ControlValidator::setupDiag() +{ + auto & d = diag_updater_; + d.setHardwareID("control_validator"); + + std::string ns = "control_validation_"; + d.add(ns + "max_distance_deviation", [&](auto & stat) { + setStatus( + stat, validation_status_.is_valid_max_distance_deviation, + "control output is deviated from trajectory"); + }); +} + +bool ControlValidator::isDataReady() +{ + const auto waiting = [this](const auto s) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", s); + return false; + }; + + if (!current_kinematics_) { + return waiting("current_kinematics_"); + } + if (!current_reference_trajectory_) { + return waiting("current_reference_trajectory_"); + } + if (!current_predicted_trajectory_) { + return waiting("current_predicted_trajectory_"); + } + return true; +} + +void ControlValidator::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) +{ + current_reference_trajectory_ = msg; + + return; +} + +void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) +{ + current_predicted_trajectory_ = msg; + + if (!isDataReady()) return; + + debug_pose_publisher_->clearMarkers(); + + validate(*current_predicted_trajectory_); + + diag_updater_.force_update(); + + // for debug + publishDebugInfo(); + displayStatus(); +} + +void ControlValidator::publishDebugInfo() +{ + validation_status_.stamp = get_clock()->now(); + pub_status_->publish(validation_status_); + + if (!isAllValid(validation_status_)) { + geometry_msgs::msg::Pose front_pose = current_kinematics_->pose.pose; + shiftPose(front_pose, vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m); + debug_pose_publisher_->pushVirtualWall(front_pose); + debug_pose_publisher_->pushWarningMsg(front_pose, "INVALID CONTROL"); + } + debug_pose_publisher_->publish(); +} + +void ControlValidator::validate(const Trajectory & predicted_trajectory) +{ + if (predicted_trajectory.points.size() < 2) { + RCLCPP_ERROR(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); + return; + } + + auto & s = validation_status_; + + s.is_valid_max_distance_deviation = checkValidMaxDistanceDeviation(predicted_trajectory); + + s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1; +} + +bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory) +{ + validation_status_.max_distance_deviation = + calcMaxLateralDistance(*current_reference_trajectory_, predicted_trajectory); + if ( + validation_status_.max_distance_deviation > + validation_params_.max_distance_deviation_threshold) { + return false; + } + return true; +} + +bool ControlValidator::isAllValid(const ControlValidatorStatus & s) +{ + return s.is_valid_max_distance_deviation; +} + +void ControlValidator::displayStatus() +{ + if (!display_on_terminal_) return; + rclcpp::Clock clock{RCL_ROS_TIME}; + + const auto warn = [this, &clock](const bool status, const std::string & msg) { + if (!status) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "%s", msg.c_str()); + } + }; + + const auto & s = validation_status_; + + warn(s.is_valid_max_distance_deviation, "planning trajectory is too far from ego!!"); +} + +} // namespace control_validator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(control_validator::ControlValidator) diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp new file mode 100644 index 0000000000000..c94c22c807648 --- /dev/null +++ b/control/control_validator/src/debug_marker.cpp @@ -0,0 +1,101 @@ +// 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. + +#include "control_validator/debug_marker.hpp" + +#include +#include + +#include +#include +#include + +using visualization_msgs::msg::Marker; + +ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcpp::Node * node) +: node_(node) +{ + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); + + virtual_wall_pub_ = + node_->create_publisher("~/virtual_wall", 1); +} + +void ControlValidatorDebugMarkerPublisher::clearMarkers() +{ + marker_array_.markers.clear(); + marker_array_virtual_wall_.markers.clear(); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) +{ + pushPoseMarker(p.pose, ns, id); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) +{ + using tier4_autoware_utils::createMarkerColor; + + // append arrow marker + std_msgs::msg::ColorRGBA color; + if (id == 0) // Red + { + color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + } + if (id == 1) // Green + { + color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + } + if (id == 2) // Blue + { + color = createMarkerColor(0.0, 0.0, 1.0, 0.999); + } + Marker marker = tier4_autoware_utils::createDefaultMarker( + "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, + tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose = pose; + + marker_array_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushWarningMsg( + const geometry_msgs::msg::Pose & pose, const std::string & msg) +{ + visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( + "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, + tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), + tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose = pose; + marker.text = msg; + marker_array_virtual_wall_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) +{ + const auto now = node_->get_clock()->now(); + const auto stop_wall_marker = + motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); + tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); +} + +void ControlValidatorDebugMarkerPublisher::publish() +{ + debug_viz_pub_->publish(marker_array_); + virtual_wall_pub_->publish(marker_array_virtual_wall_); +} diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp new file mode 100644 index 0000000000000..88b8431b07af0 --- /dev/null +++ b/control/control_validator/src/utils.cpp @@ -0,0 +1,164 @@ +// 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 "control_validator/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace control_validator +{ + +void shiftPose(Pose & pose, double longitudinal) +{ + const auto yaw = tf2::getYaw(pose.orientation); + pose.position.x += std::cos(yaw) * longitudinal; + pose.position.y += std::sin(yaw) * longitudinal; +} + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory) +{ + const auto point_to_interpolate = + motion_utils::calcInterpolatedPoint(convertToTrajectory(predicted_trajectory), reference_pose); + modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); +} + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) +{ + TrajectoryPoints reversed_trajectory_points(trajectory_points.size()); + std::reverse_copy( + trajectory_points.begin(), trajectory_points.end(), + std::back_inserter(reversed_trajectory_points)); + return reversed_trajectory_points; +} + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points) +{ + bool predicted_trajectory_point_removed = false; + for (const auto & point : predicted_trajectory_points) { + if ( + motion_utils::calcLongitudinalOffsetToSegment(trajectory_points, 0, point.pose.position) < + 0.0) { + modified_trajectory_points.erase(modified_trajectory_points.begin()); + + predicted_trajectory_point_removed = true; + } else { + break; + } + } + + return predicted_trajectory_point_removed; +} + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory) +{ + const auto last_seg_length = motion_utils::calcSignedArcLength( + trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1); + + // If no overlapping between trajectory and predicted_trajectory, return empty trajectory + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + // OR + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + const bool & is_p_n_before_t1 = + motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; + const bool & is_p1_behind_t_n = motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, trajectory.points.size() - 2, + predicted_trajectory.points.front().pose.position) - + last_seg_length > + 0.0; + const bool is_no_overlapping = (is_p_n_before_t1 || is_p1_behind_t_n); + + if (is_no_overlapping) { + return Trajectory(); + } + + auto modified_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto predicted_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto trajectory_points = convertToTrajectoryPointArray(trajectory); + + // If first point of predicted_trajectory is in front of start of trajectory, erase points which + // are in front of trajectory start point and insert pNew along the predicted_trajectory + // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN + // trajectory: t1--------//------tN + // ↓ + // predicted_trajectory: 　　　　 tNew--p3----//------pN + // trajectory: t1--------//------tN + auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + trajectory_points, modified_trajectory_points, predicted_trajectory_points); + + if (predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points); + } + + // If last point of predicted_trajectory is behind of end of trajectory, erase points which are + // behind trajectory last point and insert pNew along the predicted_trajectory + // predicted_trajectory: 　　　　p1-----//------pN-2-----pN-1-----pN + // trajectory: t1-----//-----tN-1--tN + // ↓ + // predicted_trajectory: p1-----//------pN-2-pNew + // trajectory: t1-----//-----tN-1--tN + + auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points); + auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points); + auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points); + + auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + reversed_trajectory_points, reversed_modified_trajectory_points, + reversed_predicted_trajectory_points); + + if (reversed_predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + reversed_modified_trajectory_points, reversed_trajectory_points.front().pose, + reversed_predicted_trajectory_points); + } + + return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points)); +} + +double calcMaxLateralDistance( + const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory) +{ + const auto alined_predicted_trajectory = + alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); + double max_dist = 0; + for (const auto & point : alined_predicted_trajectory.points) { + const auto p0 = tier4_autoware_utils::getPoint(point); + // find nearest segment + const size_t nearest_segment_idx = + motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); + double temp_dist = + motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx); + if (temp_dist > max_dist) { + max_dist = temp_dist; + } + } + return max_dist; +} + +} // namespace control_validator diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml index 8796e026b0e69..8d48948a2d133 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + joy_type: DS4 update_rate: 10.0 accel_ratio: 3.0 brake_ratio: 5.0 diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json new file mode 100644 index 0000000000000..c67c575a6bd41 --- /dev/null +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -0,0 +1,118 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Joy Controller node", + "type": "object", + "definitions": { + "joy_controller": { + "type": "object", + "properties": { + "joy_type": { + "type": "string", + "description": "Joy controller type", + "default": "DS4", + "enum": ["P65", "DS4", "G29"] + }, + "update_rate": { + "type": "number", + "description": "Update rate to publish control commands", + "default": "10.0", + "exclusiveMinimum": 0 + }, + "accel_ratio": { + "type": "number", + "description": "Ratio to calculate acceleration (commanded acceleration is ratio * operation)", + "default": "3.0" + }, + "brake_ratio": { + "type": "number", + "description": "Ratio to calculate deceleration (commanded acceleration is -ratio * operation)", + "default": "5.0" + }, + "steer_ratio": { + "type": "number", + "description": "Ratio to calculate deceleration (commanded steer is ratio * operation)", + "default": "0.5" + }, + "steering_angle_velocity": { + "type": "number", + "description": "Steering angle velocity for operation", + "default": "0.1" + }, + "accel_sensitivity": { + "type": "number", + "description": "Sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity))", + "default": "1.0", + "maximum": 1.0, + "exclusiveMinimum": 0.0 + }, + "brake_sensitivity": { + "type": "number", + "description": "Sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity))", + "default": "1.0", + "maximum": 1.0, + "exclusiveMinimum": 0.0 + }, + "control_command": { + "type": "object", + "properties": { + "velocity_gain": { + "type": "number", + "description": "Ratio to calculate velocity by acceleration", + "default": "3.0" + }, + "max_forward_velocity": { + "type": "number", + "description": "Absolute max velocity to go forward", + "default": "20.0", + "minimum": 0 + }, + "max_backward_velocity": { + "type": "number", + "description": "Absolute max velocity to go backward", + "default": "3.0", + "minimum": 0 + }, + "backward_accel_ratio": { + "type": "number", + "description": "Ratio to calculate deceleration (commanded acceleration is -ratio * operation)", + "default": "1.0" + } + }, + "required": [ + "velocity_gain", + "max_forward_velocity", + "max_backward_velocity", + "backward_accel_ratio" + ], + "additionalProperties": false + } + }, + "required": [ + "joy_type", + "update_rate", + "accel_ratio", + "brake_ratio", + "steer_ratio", + "steering_angle_velocity", + "accel_sensitivity", + "brake_sensitivity", + "control_command" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/joy_controller" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index e0609997737b8..5148d6a998f50 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -8,6 +8,7 @@ This package includes the following features: - **Lane Departure**: Check if ego vehicle is going to be out of lane boundaries based on output from control module (predicted trajectory). - **Trajectory Deviation**: Check if ego vehicle's pose does not deviate from the trajectory. Checking lateral, longitudinal and yaw deviation. +- **Road Border Departure**: Check if ego vehicle's footprint, generated from the control's output, extends beyond the road border. ## Inner-workings / Algorithms @@ -62,10 +63,15 @@ This package includes the following features: ### Node Parameters -| Name | Type | Description | Default value | -| :---------------- | :----- | :---------------------------- | :------------ | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | +| road_border_departure_checker | bool | Flag for checking if the vehicle will departs the road border | False | ### Core Parameters diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 59e25cbc5d86e..008832b1cab21 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -7,6 +7,7 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false + road_border_departure_checker: false # Core footprint_margin_scale: 1.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 244e75210cec3..9822fd820dc3c 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -80,6 +80,7 @@ struct Output std::map processing_time_map{}; bool will_leave_lane{}; bool is_out_of_lane{}; + bool will_cross_road_border{}; PoseDeviation trajectory_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; @@ -135,6 +136,13 @@ class LaneDepartureChecker static bool willLeaveLane( const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); + + static bool willCrossRoadBorder( + const lanelet::ConstLanelets & candidate_lanelets, + const std::vector & vehicle_footprints); + + static bool isCrossingRoadBorder( + const lanelet::BasicLineString2d & road_border, const std::vector & footprints); }; } // namespace lane_departure_checker diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index 85fd73c63f544..f7f7bcfda7d51 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -51,6 +51,7 @@ struct NodeParam bool include_left_lanes; bool include_opposite_lanes; bool include_conflicting_lanes; + bool road_border_departure_checker; }; class LaneDepartureCheckerNode : public rclcpp::Node diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index d4e845ec97d82..1059e86cadc6d 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -5,6 +5,7 @@ 0.1.0 The lane_departure_checker package Kyoichi Sugahara + Makoto Kurihara Apache License 2.0 ament_cmake_auto diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 1a63fb3eb395b..f1d8d75452df1 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -39,6 +39,16 @@ namespace using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Point; + +Point fromVector2dToMsg(const Eigen::Vector2d & point) +{ + Point msg{}; + msg.x = point.x(); + msg.y = point.y(); + msg.z = 0.0; + return msg; +} double calcBrakingDistance( const double abs_velocity, const double max_deceleration, const double delay_time) @@ -57,6 +67,27 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } +bool isCrossingWithRoadBorder( + const lanelet::BasicLineString2d & road_border, const std::vector & footprints) +{ + for (auto & footprint : footprints) { + for (size_t i = 0; i < footprint.size() - 1; ++i) { + auto footprint1 = footprint.at(i).to_3d(); + auto footprint2 = footprint.at(i + 1).to_3d(); + for (size_t i = 0; i < road_border.size() - 1; ++i) { + auto road_border1 = road_border.at(i); + auto road_border2 = road_border.at(i + 1); + if (tier4_autoware_utils::intersect( + tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), + fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) { + return true; + } + } + } + } + return false; +} + LinearRing2d createHullFromFootprints(const std::vector & footprints) { MultiPoint2d combined; @@ -141,6 +172,10 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); + output.will_cross_road_border = + willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints); + output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true); + return output; } @@ -298,4 +333,34 @@ bool LaneDepartureChecker::isOutOfLane( return false; } + +bool LaneDepartureChecker::willCrossRoadBorder( + const lanelet::ConstLanelets & candidate_lanelets, + const std::vector & vehicle_footprints) +{ + for (const auto & candidate_lanelet : candidate_lanelets) { + const std::string r_type = + candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); + if (r_type == "road_border") { + if (isCrossingWithRoadBorder( + candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { + // std::cerr << "The crossed road_border's line string id: " + // << candidate_lanelet.rightBound().id() << std::endl; + return true; + } + } + const std::string l_type = + candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); + if (l_type == "road_border") { + if (isCrossingWithRoadBorder( + candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { + // std::cerr << "The crossed road_border's line string id: " + // << candidate_lanelet.leftBound().id() << std::endl; + return true; + } + } + } + return false; +} + } // namespace lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index ed9149cd6df2d..20d535a82bfa1 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -132,6 +132,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o node_param_.include_left_lanes = declare_parameter("include_left_lanes"); node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); + node_param_.road_border_departure_checker = + declare_parameter("road_border_departure_checker"); // Vehicle Info const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -417,6 +419,11 @@ void LaneDepartureCheckerNode::checkLaneDeparture( msg = "vehicle is out of lane"; } + if (output_.will_cross_road_border && node_param_.road_border_departure_checker) { + level = DiagStatus::ERROR; + msg = "vehicle will cross road border"; + } + stat.summary(level, msg); } diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt index 2437e8cacb386..e4a3683ea1fdc 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/mpc_lateral_controller/CMakeLists.txt @@ -17,7 +17,7 @@ ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED src/mpc_trajectory.cpp src/mpc_utils.cpp src/qp_solver/qp_solver_osqp.cpp - src/qp_solver/qp_solver_unconstr_fast.cpp + src/qp_solver/qp_solver_unconstraint_fast.cpp src/vehicle_model/vehicle_model_bicycle_dynamics.cpp src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp src/vehicle_model/vehicle_model_bicycle_kinematics.cpp 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 0d144726a0866..0854340ed24a5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -379,6 +379,14 @@ class MPC const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; + /** + * @brief calculate steering rate limit along with the target trajectory + * @param reference_trajectory The reference trajectory. + * @param current_velocity current velocity of ego. + */ + VectorXd calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const; + //!< @brief logging with warn and return false template inline bool fail_warn_throttle(Args &&... args) const diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp rename to control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index e9ef492c80c88..3a6bd2b25832b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" @@ -55,4 +55,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_ +#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/mpc_lateral_controller/model_predictive_control_algorithm.md index 9dec0def2e477..fa214abc9a564 100644 --- a/control/mpc_lateral_controller/model_predictive_control_algorithm.md +++ b/control/mpc_lateral_controller/model_predictive_control_algorithm.md @@ -125,7 +125,7 @@ Substituting equation (8) into equation (9) and tidying up the equation for $U$. $$ \begin{align} J(U) &= (H(Fx_{0}+GU+SW)-Y_{ref})^{T}Q(H(Fx_{0}+GU+SW)-Y_{ref})+(U-U_{ref})^{T}R(U-U_{ref}) \\ -& =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Yref)^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10} +& =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Y_{ref})^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10} \end{align} $$ @@ -182,7 +182,7 @@ Where $\kappa_{r}\left(s\right)$ is the curvature along the trajectory parametri There are three expressions in the update equations that are subject to linear approximation: the lateral deviation (or lateral coordinate) $y$, the heading angle (or the heading angle error) $\theta$, and the steering $\delta$. We can make a small angle assumption on the heading angle $\theta$. -In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackerman steering expression can be written as; +In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackermann steering expression can be written as; $$ \begin{align} @@ -344,7 +344,7 @@ $$ \end{align} $$ -Discretisizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex +We discretize $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt, and the resulting constraint become linear and convex $$ \begin{align} diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8e99880b68050..643f8a6f91023 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -359,9 +359,6 @@ std::pair MPC::updateStateForDelayCompensation( MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; - // for (const auto & tt : traj.relative_time) { - // std::cerr << "traj.relative_time = " << tt << std::endl; - // } for (size_t i = 0; i < m_input_buffer.size(); ++i) { double k, v = 0.0; try { @@ -573,47 +570,16 @@ std::pair MPC::executeOptimization( A(i, i - 1) = -1.0; } - const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; - const auto get_adaptive_steer_rate_lim = [&](const double curvature, const double velocity) { - if (is_vehicle_stopped) { - return std::numeric_limits::max(); - } - - double steer_rate_lim_by_curvature = m_steer_rate_lim_map_by_curvature.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_curvature) { - if (std::abs(curvature) <= steer_rate_lim_info.first) { - steer_rate_lim_by_curvature = steer_rate_lim_info.second; - break; - } - } - - double steer_rate_lim_by_velocity = m_steer_rate_lim_map_by_velocity.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_velocity) { - if (std::abs(velocity) <= steer_rate_lim_info.first) { - steer_rate_lim_by_velocity = steer_rate_lim_info.second; - break; - } - } - - return std::min(steer_rate_lim_by_curvature, steer_rate_lim_by_velocity); - }; - + // steering angle limit VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim); // min steering angle VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle - VectorXd lbA(DIM_U_N); - VectorXd ubA(DIM_U_N); - for (int i = 0; i < DIM_U_N; ++i) { - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(i), traj.vx.at(i)); - const double adaptive_delta_steer_lim = adaptive_steer_rate_lim * prediction_dt; - lbA(i) = -adaptive_delta_steer_lim; - ubA(i) = adaptive_delta_steer_lim; - } - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(0), traj.vx.at(0)); - lbA(0, 0) = m_raw_steer_cmd_prev - adaptive_steer_rate_lim * m_ctrl_period; - ubA(0, 0) = m_raw_steer_cmd_prev + adaptive_steer_rate_lim * m_ctrl_period; + // steering angle rate limit + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); + VectorXd ubA = steer_rate_limits * prediction_dt; + VectorXd lbA = -steer_rate_limits * prediction_dt; + ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; + lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period; auto t_start = std::chrono::system_clock::now(); bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex); @@ -761,6 +727,58 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } +VectorXd MPC::calcSteerRateLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const +{ + const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { + std::vector reference, limits; + for (const auto & p : steer_rate_limit_map) { + reference.push_back(p.first); + limits.push_back(p.second); + } + + // If the speed is out of range of the reference, apply zero-order hold. + if (current <= reference.front()) { + return limits.front(); + } + if (current >= reference.back()) { + return limits.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < reference.size() - 1; ++i) { + if (reference.at(i) <= current && current <= reference.at(i + 1)) { + auto ratio = + (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i)); + return interp; + } + } + + std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command " + "filter is not working. Please check the code." + << std::endl; + return reference.back(); + }; + + // when the vehicle is stopped, no steering rate limit. + const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; + if (is_vehicle_stopped) { + return VectorXd::Zero(m_param.prediction_horizon); + } + + // calculate steering rate limit + VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); + for (int i = 0; i < m_param.prediction_horizon; ++i) { + const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i)); + const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i)); + steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity); + } + + return steer_rate_limits; +} + 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 c7fbce7411f1e..af680b5050037 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -16,7 +16,7 @@ #include "motion_utils/motion_utils.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #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" diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index dba803e8f5efc..7c76d67e75aa3 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -72,12 +72,10 @@ bool QPSolverOSQP::solve( // polish status: successful (1), unperformed (0), (-1) unsuccessful int status_polish = std::get<2>(result); - if (status_polish == -1) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unsuccessful)", status_polish); - return false; - } - if (status_polish == 0) { - RCLCPP_WARN(logger_, "osqp status_polish = %d (unperformed)", status_polish); + if (status_polish == -1 || status_polish == 0) { + const auto s = (status_polish == 0) ? "Polish process is not performed in osqp." + : "Polish process failed in osqp."; + RCLCPP_INFO(logger_, "%s The required accuracy is met, but the solution can be inaccurate.", s); return true; } return true; diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp similarity index 94% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp rename to control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index 3c54ba6ffdc48..b0357138cd646 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstr_fast.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 72066003696d8..6f8a6fb598058 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -15,7 +15,7 @@ #include "gtest/gtest.h" #include "mpc_lateral_controller/mpc.hpp" #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp" +#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #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" @@ -252,9 +252,8 @@ TEST_F(MPCTest, OsqpCalculate) AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; - // with OSQP this function returns false despite finding correct solutions const auto odom = makeOdometry(pose_zero, default_velocity); - EXPECT_FALSE(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); } 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 5584e1e79e971..8685d6264993b 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 @@ -72,11 +72,12 @@ double getPitchByTraj( double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); /** - * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for - * delayed time + * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and + * acceleration for delayed time */ Pose calcPoseAfterTimeDelay( - const Pose & current_pose, const double delay_time, const double current_vel); + const Pose & current_pose, const double delay_time, const double current_vel, + const double current_acc); /** * @brief apply linear interpolation to orientation diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 8cbf02b90ce51..70cdbcaf17bd2 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -129,11 +129,23 @@ double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint } Pose calcPoseAfterTimeDelay( - const Pose & current_pose, const double delay_time, const double current_vel) + const Pose & current_pose, const double delay_time, const double current_vel, + const double current_acc) { + if (delay_time <= 0.0) { + return current_pose; + } + + // check time to stop + const double time_to_stop = -current_vel / current_acc; + + const double delay_time_calculation = + time_to_stop > 0.0 && time_to_stop < delay_time ? time_to_stop : delay_time; // simple linear prediction const double yaw = tf2::getYaw(current_pose.orientation); - const double running_distance = delay_time * current_vel; + const double running_distance = delay_time_calculation * current_vel + 0.5 * current_acc * + delay_time_calculation * + delay_time_calculation; const double dx = running_distance * std::cos(yaw); const double dy = running_distance * std::sin(yaw); diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 23c4bcc857cd4..337a63bc7dc76 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -626,7 +626,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( Motion target_motion{}; if (m_control_state == ControlState::DRIVE) { const auto target_pose = longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel); + current_pose, m_delay_compensation_time, current_vel, current_acc); const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, 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 ef38533376446..d9fc404ce6abe 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -197,25 +197,30 @@ TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) quaternion_tf.setRPY(0.0, 0.0, 0.0); current_pose.orientation = tf2::toMsg(quaternion_tf); - // With a delay and/or a velocity of 0.0 there is no change of position + // With a delay acceleration and/or a velocity of 0.0 there is no change of position double delay_time = 0.0; double current_vel = 0.0; + double current_acc = 0.0; Pose delayed_pose = - longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); delay_time = 1.0; current_vel = 0.0; - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + current_acc = 0.0; + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); delay_time = 0.0; current_vel = 1.0; - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + current_acc = 0.0; + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); @@ -223,46 +228,89 @@ TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) // With both delay and velocity: change of position delay_time = 1.0; current_vel = 1.0; - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + current_acc = 0.0; + + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + // With all, acceleration, delay and velocity: change of position + delay_time = 1.0; + current_vel = 1.0; + current_acc = 1.0; + + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); + EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); + EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); + // Vary the yaw quaternion_tf.setRPY(0.0, 0.0, M_PI); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x - current_vel * delay_time, abs_err); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x - current_vel * delay_time - + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); quaternion_tf.setRPY(0.0, 0.0, M_PI_2); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y + current_vel * delay_time, abs_err); + EXPECT_NEAR( + delayed_pose.position.y, + current_pose.position.y + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); quaternion_tf.setRPY(0.0, 0.0, -M_PI_2); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err); - EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y - current_vel * delay_time, abs_err); + EXPECT_NEAR( + delayed_pose.position.y, + current_pose.position.y - current_vel * delay_time - + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); // Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI quaternion_tf.setRPY(0.0, M_PI_4, 0.0); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); // Vary the roll : no effect quaternion_tf.setRPY(M_PI_2, 0.0, 0.0); current_pose.orientation = tf2::toMsg(quaternion_tf); - delayed_pose = longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel); - EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time, abs_err); + delayed_pose = + longitudinal_utils::calcPoseAfterTimeDelay(current_pose, delay_time, current_vel, current_acc); + EXPECT_NEAR( + delayed_pose.position.x, + current_pose.position.x + current_vel * delay_time + + 0.5 * current_acc * delay_time * delay_time, + abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err); EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err); } diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp index 717c1ecac6c77..99b1a82ab2dce 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp @@ -47,7 +47,7 @@ namespace pure_pursuit class PurePursuit { public: - PurePursuit() : lookahead_distance_(0.0), clst_thr_dist_(3.0), clst_thr_ang_(M_PI / 4) {} + PurePursuit() : lookahead_distance_(0.0), closest_thr_dist_(3.0), closest_thr_ang_(M_PI / 4) {} ~PurePursuit() = default; rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit"); @@ -55,10 +55,10 @@ class PurePursuit void setCurrentPose(const geometry_msgs::msg::Pose & msg); void setWaypoints(const std::vector & msg); void setLookaheadDistance(double ld) { lookahead_distance_ = ld; } - void setClosestThreshold(double clst_thr_dist, double clst_thr_ang) + void setClosestThreshold(double closest_thr_dist, double closest_thr_ang) { - clst_thr_dist_ = clst_thr_dist; - clst_thr_ang_ = clst_thr_ang; + closest_thr_dist_ = closest_thr_dist; + closest_thr_ang_ = closest_thr_ang; } // getter @@ -74,7 +74,7 @@ class PurePursuit geometry_msgs::msg::Point loc_next_tgt_; // variables got from outside - double lookahead_distance_, clst_thr_dist_, clst_thr_ang_; + double lookahead_distance_, closest_thr_dist_, closest_thr_ang_; std::shared_ptr> curr_wps_ptr_; std::shared_ptr curr_pose_ptr_; diff --git a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp index 0b186a0c4514b..0ee96e970782c 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp @@ -55,16 +55,17 @@ std::pair PurePursuit::run() return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - auto clst_pair = planning_utils::findClosestIdxWithDistAngThr( - *curr_wps_ptr_, *curr_pose_ptr_, clst_thr_dist_, clst_thr_ang_); + auto closest_pair = planning_utils::findClosestIdxWithDistAngThr( + *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_); - if (!clst_pair.first) { + if (!closest_pair.first) { RCLCPP_WARN( - logger, "cannot find, curr_bool: %d, clst_idx: %d", clst_pair.first, clst_pair.second); + logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first, + closest_pair.second); return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - int32_t next_wp_idx = findNextPointIdx(clst_pair.second); + int32_t next_wp_idx = findNextPointIdx(closest_pair.second); if (next_wp_idx == -1) { RCLCPP_WARN(logger, "lost next waypoint"); return std::make_pair(false, std::numeric_limits::quiet_NaN()); diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt index 81ccac1ea8b66..c5db663ccc77e 100644 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ b/control/vehicle_cmd_gate/CMakeLists.txt @@ -13,13 +13,31 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED rclcpp_components_register_node(vehicle_cmd_gate_node PLUGIN "vehicle_cmd_gate::VehicleCmdGate" - EXECUTABLE vehicle_cmd_gate + EXECUTABLE vehicle_cmd_gate_exe ) +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/IsFilterActivated.msg" + DEPENDENCIES builtin_interfaces +) + +# to use same package defined message +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(vehicle_cmd_gate_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}") +endif() + + if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_vehicle_cmd_gate test/src/test_main.cpp test/src/test_vehicle_cmd_filter.cpp + test/src/test_filter_in_vehicle_cmd_gate_node.cpp ) ament_target_dependencies(test_vehicle_cmd_gate rclcpp diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index a8ac8c46ae782..231c2c5022138 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -43,26 +43,38 @@ ## Parameters -| Parameter | Type | Description | -| ------------------------------------------- | ------ | --------------------------------------------------------------------------- | -| `update_period` | double | update period | -| `use_emergency_handling` | bool | true when emergency handler is used | -| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop | -| `system_emergency_heartbeat_timeout` | double | timeout for system emergency | -| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | -| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | -| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | -| `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service | -| `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) | -| `nominal.lon_acc_lim` | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) | -| `nominal.lon_jerk_lim` | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode) | -| `nominal.lat_acc_lim` | double | limit of lateral acceleration (activated in AUTONOMOUS operation mode) | -| `nominal.lat_jerk_lim` | double | limit of lateral jerk (activated in AUTONOMOUS operation mode) | -| `on_transition.vel_lim` | double | limit of longitudinal velocity (activated in TRANSITION operation mode) | -| `on_transition.lon_acc_lim` | double | limit of longitudinal acceleration (activated in TRANSITION operation mode) | -| `on_transition.lon_jerk_lim` | double | limit of longitudinal jerk (activated in TRANSITION operation mode) | -| `on_transition.lat_acc_lim` | double | limit of lateral acceleration (activated in TRANSITION operation mode) | -| `on_transition.lat_jerk_lim` | double | limit of lateral jerk (activated in TRANSITION operation mode) | +| Parameter | Type | Description | +| ------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `update_period` | double | update period | +| `use_emergency_handling` | bool | true when emergency handler is used | +| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop | +| `system_emergency_heartbeat_timeout` | double | timeout for system emergency | +| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | +| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | +| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | +| `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service | +| `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) | +| `nominal.reference_speed_point` | | velocity point used as a reference when calculate control command limit (activated in AUTONOMOUS operation mode). The size of this array must be equivalent to the size of the limit array. | +| `nominal.lon_acc_lim` | | array of limits of longitudinal acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in AUTONOMOUS operation mode) | +| `nominal.lat_acc_lim` | | array of limits of lateral acceleration (activated in AUTONOMOUS operation mode) | +| `nominal.lat_jerk_lim` | | array of limits of lateral jerk (activated in AUTONOMOUS operation mode) | +| `on_transition.vel_lim` | double | limit of longitudinal velocity (activated in TRANSITION operation mode) | +| `on_transition.reference_speed_point` | | velocity point used as a reference when calculate control command limit (activated in TRANSITION operation mode). The size of this array must be equivalent to the size of the limit array. | +| `on_transition.lon_acc_lim` | | array of limits of longitudinal acceleration (activated in TRANSITION operation mode) | +| `on_transition.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in TRANSITION operation mode) | +| `on_transition.lat_acc_lim` | | array of limits of lateral acceleration (activated in TRANSITION operation mode) | +| `on_transition.lat_jerk_lim` | | array of limits of lateral jerk (activated in TRANSITION operation mode) | + +## Filter function + +This module incorporates a limitation filter to the control command right before its published. Primarily for safety, this filter restricts the output range of all control commands published through Autoware. + +The limitation values are calculated based on the 1D interpolation of the limitation array parameters. Here is an example for the longitudinal jerk limit. + +![filter-example](./image/filter.png) + +Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published. ## Assumptions / Known limits diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 7a49d3e5f82d3..92844c61f6f4f 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -5,6 +5,7 @@ use_emergency_handling: false check_external_emergency_heartbeat: false use_start_request: false + enable_cmd_limit_filter: true external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 @@ -13,15 +14,17 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 7.0 - actual_steer_diff_lim: 1.0 + reference_speed_points: [20.0, 30.0] + lon_acc_lim: [5.0, 4.0] + lon_jerk_lim: [5.0, 4.0] + lat_acc_lim: [5.0, 4.0] + lat_jerk_lim: [7.0, 6.0] + actual_steer_diff_lim: [1.0, 0.8] on_transition: vel_lim: 50.0 - lon_acc_lim: 1.0 - lon_jerk_lim: 0.5 - lat_acc_lim: 2.0 - lat_jerk_lim: 7.0 - actual_steer_diff_lim: 1.0 + reference_speed_points: [20.0, 30.0] + lon_acc_lim: [1.0, 0.9] + lon_jerk_lim: [0.5, 0.4] + lat_acc_lim: [2.0, 1.8] + lat_jerk_lim: [7.0, 6.0] + actual_steer_diff_lim: [1.0, 0.8] diff --git a/control/vehicle_cmd_gate/image/filter.png b/control/vehicle_cmd_gate/image/filter.png new file mode 100644 index 0000000000000..ed1c7f772a932 Binary files /dev/null and b/control/vehicle_cmd_gate/image/filter.png differ diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index 644e937e0718d..a6cf7cdcf8c08 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/control/vehicle_cmd_gate/msg/IsFilterActivated.msg b/control/vehicle_cmd_gate/msg/IsFilterActivated.msg new file mode 100644 index 0000000000000..fdb47c0a78f08 --- /dev/null +++ b/control/vehicle_cmd_gate/msg/IsFilterActivated.msg @@ -0,0 +1,10 @@ +builtin_interfaces/Time stamp + +bool is_activated + +# for additional information +bool is_activated_on_steering +bool is_activated_on_steering_rate +bool is_activated_on_speed +bool is_activated_on_acceleration +bool is_activated_on_jerk diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index df1de9f370cb6..36bb662fb2f53 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -13,6 +13,8 @@ ament_cmake autoware_cmake + rosidl_default_generators + autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_system_msgs @@ -33,10 +35,14 @@ tier4_vehicle_msgs vehicle_info_util + rosidl_default_runtime + ament_cmake_ros ament_lint_auto autoware_lint_common + rosidl_interface_packages + ament_cmake diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 49735da876d9a..8dec06c455868 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -24,6 +24,60 @@ VehicleCmdFilter::VehicleCmdFilter() { } +bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & p) +{ + const auto s = p.reference_speed_points.size(); + if ( + p.lon_acc_lim.size() != s || p.lon_jerk_lim.size() != s || p.lat_acc_lim.size() != s || + p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s) { + std::cerr << "VehicleCmdFilter::setParam() There is a size mismatch in the parameter. " + "Parameter initialization failed." + << std::endl; + return false; + } + + param_ = p; + return true; +} + +void VehicleCmdFilter::setLonAccLim(LimitArray v) +{ + auto tmp = param_; + tmp.lon_acc_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setLonJerkLim(LimitArray v) +{ + auto tmp = param_; + tmp.lon_jerk_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setLatAccLim(LimitArray v) +{ + auto tmp = param_; + tmp.lat_acc_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setLatJerkLim(LimitArray v) +{ + auto tmp = param_; + tmp.lat_jerk_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setActualSteerDiffLim(LimitArray v) +{ + auto tmp = param_; + tmp.actual_steer_diff_lim = v; + setParameterWithValidation(tmp); +} + +void VehicleCmdFilter::setParam(const VehicleCmdFilterParam & p) +{ + if (!setParameterWithValidation(p)) { + std::exit(EXIT_FAILURE); + } +} + void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const { input.longitudinal.speed = std::max( @@ -33,28 +87,33 @@ void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) void VehicleCmdFilter::limitLongitudinalWithAcc( const double dt, AckermannControlCommand & input) const { + const auto lon_acc_lim = getLonAccLim(); input.longitudinal.acceleration = std::max( - std::min(static_cast(input.longitudinal.acceleration), param_.lon_acc_lim), - -param_.lon_acc_lim); + std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim), -lon_acc_lim); input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, param_.lon_acc_lim * dt); + limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( const double dt, AckermannControlCommand & input) const { + const auto lon_jerk_lim = getLonJerkLim(); input.longitudinal.acceleration = limitDiff( - input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, param_.lon_jerk_lim * dt); + input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, lon_jerk_lim * dt); + input.longitudinal.jerk = + std::clamp(static_cast(input.longitudinal.jerk), -lon_jerk_lim, lon_jerk_lim); } void VehicleCmdFilter::limitLateralWithLatAcc( [[maybe_unused]] const double dt, AckermannControlCommand & input) const { + const auto lat_acc_lim = getLatAccLim(); + double latacc = calcLatAcc(input); - if (std::fabs(latacc) > param_.lat_acc_lim) { + if (std::fabs(latacc) > lat_acc_lim) { double v_sq = std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); - double steer_lim = std::atan(param_.lat_acc_lim * param_.wheel_base / v_sq); + double steer_lim = std::atan(lat_acc_lim * param_.wheel_base / v_sq); input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; } } @@ -65,8 +124,10 @@ void VehicleCmdFilter::limitLateralWithLatJerk( double curr_latacc = calcLatAcc(input); double prev_latacc = calcLatAcc(prev_cmd_); - const double latacc_max = prev_latacc + param_.lat_jerk_lim * dt; - const double latacc_min = prev_latacc - param_.lat_jerk_lim * dt; + const auto lat_jerk_lim = getLatJerkLim(); + + const double latacc_max = prev_latacc + lat_jerk_lim * dt; + const double latacc_min = prev_latacc - lat_jerk_lim * dt; if (curr_latacc > latacc_max) { input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); @@ -78,23 +139,60 @@ void VehicleCmdFilter::limitLateralWithLatJerk( void VehicleCmdFilter::limitActualSteerDiff( const double current_steer_angle, AckermannControlCommand & input) const { + const auto actual_steer_diff_lim = getSteerDiffLim(); + auto ds = input.lateral.steering_tire_angle - current_steer_angle; - ds = std::clamp(ds, -param_.actual_steer_diff_lim, param_.actual_steer_diff_lim); + ds = std::clamp(ds, -actual_steer_diff_lim, actual_steer_diff_lim); input.lateral.steering_tire_angle = current_steer_angle + ds; } +void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const +{ + // TODO(Horibe): parametrize the max steering angle. + // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration + // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. + constexpr float steer_limit = M_PI_2; + input.lateral.steering_tire_angle = + std::clamp(input.lateral.steering_tire_angle, -steer_limit, steer_limit); +} + void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & cmd) const + const double dt, const double current_steer_angle, AckermannControlCommand & cmd, + IsFilterActivated & is_activated) const { + const auto cmd_orig = cmd; + limitLateralSteer(cmd); limitLongitudinalWithJerk(dt, cmd); limitLongitudinalWithAcc(dt, cmd); limitLongitudinalWithVel(cmd); limitLateralWithLatJerk(dt, cmd); limitLateralWithLatAcc(dt, cmd); limitActualSteerDiff(current_steer_angle, cmd); + + is_activated = checkIsActivated(cmd, cmd_orig); return; } +IsFilterActivated VehicleCmdFilter::checkIsActivated( + const AckermannControlCommand & c1, const AckermannControlCommand & c2, const double tol) +{ + IsFilterActivated msg; + msg.is_activated_on_steering = + std::abs(c1.lateral.steering_tire_angle - c2.lateral.steering_tire_angle) > tol; + msg.is_activated_on_steering_rate = + std::abs(c1.lateral.steering_tire_rotation_rate - c2.lateral.steering_tire_rotation_rate) > tol; + msg.is_activated_on_speed = std::abs(c1.longitudinal.speed - c2.longitudinal.speed) > tol; + msg.is_activated_on_acceleration = + std::abs(c1.longitudinal.acceleration - c2.longitudinal.acceleration) > tol; + msg.is_activated_on_jerk = std::abs(c1.longitudinal.jerk - c2.longitudinal.jerk) > tol; + + msg.is_activated = + (msg.is_activated_on_steering || msg.is_activated_on_steering_rate || + msg.is_activated_on_speed || msg.is_activated_on_acceleration || msg.is_activated_on_jerk); + + return msg; +} + double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc) const { const double v_sq = std::max(v * v, 0.001); @@ -114,4 +212,56 @@ double VehicleCmdFilter::limitDiff( return prev + diff; } +double VehicleCmdFilter::interpolateFromSpeed(const LimitArray & limits) const +{ + // Consider only for the positive velocities. + const auto current = std::abs(current_speed_); + const auto reference = param_.reference_speed_points; + + // If the speed is out of range of the reference, apply zero-order hold. + if (current <= reference.front()) { + return limits.front(); + } + if (current >= reference.back()) { + return limits.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < reference.size() - 1; ++i) { + if (reference.at(i) <= current && current <= reference.at(i + 1)) { + auto ratio = + (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i)); + return interp; + } + } + + std::cerr << "VehicleCmdFilter::interpolateFromSpeed() interpolation logic is broken. Command " + "filter is not working. Please check the code." + << std::endl; + return reference.back(); +} + +double VehicleCmdFilter::getLonAccLim() const +{ + return interpolateFromSpeed(param_.lon_acc_lim); +} +double VehicleCmdFilter::getLonJerkLim() const +{ + return interpolateFromSpeed(param_.lon_jerk_lim); +} +double VehicleCmdFilter::getLatAccLim() const +{ + return interpolateFromSpeed(param_.lat_acc_lim); +} +double VehicleCmdFilter::getLatJerkLim() const +{ + return interpolateFromSpeed(param_.lat_jerk_lim); +} +double VehicleCmdFilter::getSteerDiffLim() const +{ + return interpolateFromSpeed(param_.actual_steer_diff_lim); +} + } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index 00acb50080cfe..eb85fcbeb8606 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -16,22 +16,28 @@ #define VEHICLE_CMD_FILTER_HPP_ #include +#include #include +#include + namespace vehicle_cmd_gate { using autoware_auto_control_msgs::msg::AckermannControlCommand; +using vehicle_cmd_gate::msg::IsFilterActivated; +using LimitArray = std::vector; struct VehicleCmdFilterParam { double wheel_base; double vel_lim; - double lon_acc_lim; - double lon_jerk_lim; - double lat_acc_lim; - double lat_jerk_lim; - double actual_steer_diff_lim; + LimitArray reference_speed_points; + LimitArray lon_acc_lim; + LimitArray lon_jerk_lim; + LimitArray lat_acc_lim; + LimitArray lat_jerk_lim; + LimitArray actual_steer_diff_lim; }; class VehicleCmdFilter { @@ -41,12 +47,13 @@ class VehicleCmdFilter void setWheelBase(double v) { param_.wheel_base = v; } void setVelLim(double v) { param_.vel_lim = v; } - void setLonAccLim(double v) { param_.lon_acc_lim = v; } - void setLonJerkLim(double v) { param_.lon_jerk_lim = v; } - void setLatAccLim(double v) { param_.lat_acc_lim = v; } - void setLatJerkLim(double v) { param_.lat_jerk_lim = v; } - void setActualSteerDiffLim(double v) { param_.actual_steer_diff_lim = v; } - void setParam(const VehicleCmdFilterParam & p) { param_ = p; } + void setLonAccLim(LimitArray v); + void setLonJerkLim(LimitArray v); + void setLatAccLim(LimitArray v); + void setLatJerkLim(LimitArray v); + void setActualSteerDiffLim(LimitArray v); + void setCurrentSpeed(double v) { current_speed_ = v; } + void setParam(const VehicleCmdFilterParam & p); void setPrevCmd(const AckermannControlCommand & v) { prev_cmd_ = v; } void limitLongitudinalWithVel(AckermannControlCommand & input) const; @@ -56,18 +63,33 @@ class VehicleCmdFilter void limitLateralWithLatJerk(const double dt, AckermannControlCommand & input) const; void limitActualSteerDiff( const double current_steer_angle, AckermannControlCommand & input) const; + void limitLateralSteer(AckermannControlCommand & input) const; void filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & input) const; + const double dt, const double current_steer_angle, AckermannControlCommand & input, + IsFilterActivated & is_activated) const; + static IsFilterActivated checkIsActivated( + const AckermannControlCommand & c1, const AckermannControlCommand & c2, + const double tol = 1.0e-3); AckermannControlCommand getPrevCmd() { return prev_cmd_; } private: VehicleCmdFilterParam param_; AckermannControlCommand prev_cmd_; + double current_speed_ = 0.0; + + bool setParameterWithValidation(const VehicleCmdFilterParam & p); double calcLatAcc(const AckermannControlCommand & cmd) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; + + double interpolateFromSpeed(const LimitArray & limits) const; + double getLonAccLim() const; + double getLonJerkLim() const; + double getLatAccLim() const; + double getLatJerkLim() const; + double getSteerDiffLim() const; }; } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index dc1d9ec1fedb8..9030521b6b334 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace vehicle_cmd_gate { @@ -70,6 +71,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) pub_external_emergency_ = create_publisher("output/external_emergency", durable_qos); operation_mode_pub_ = create_publisher("output/operation_mode", durable_qos); + is_filter_activated_pub_ = + create_publisher("~/is_filter_activated", durable_qos); + // Subscriber external_emergency_stop_heartbeat_sub_ = create_subscription( "input/external_emergency_stop_heartbeat", 1, @@ -79,7 +83,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_sub_ = create_subscription( "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); kinematics_sub_ = create_subscription( - "input/kinematics", 1, [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; }); + "/localization/kinematic_state", 1, + [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; }); acc_sub_ = create_subscription( "input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) { current_acceleration_ = msg->accel.accel.linear.x; @@ -150,6 +155,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) moderate_stop_service_acceleration_ = declare_parameter("moderate_stop_service_acceleration"); stop_check_duration_ = declare_parameter("stop_check_duration"); + enable_cmd_limit_filter_ = declare_parameter("enable_cmd_limit_filter"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -157,11 +163,14 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; p.vel_lim = declare_parameter("nominal.vel_lim"); - p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim"); - p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim"); - p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim"); - p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim"); - p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim"); + p.reference_speed_points = + declare_parameter>("nominal.reference_speed_points"); + p.lon_acc_lim = declare_parameter>("nominal.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter>("nominal.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter>("nominal.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter>("nominal.lat_jerk_lim"); + p.actual_steer_diff_lim = + declare_parameter>("nominal.actual_steer_diff_lim"); filter_.setParam(p); } @@ -169,11 +178,14 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; p.vel_lim = declare_parameter("on_transition.vel_lim"); - p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim"); - p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim"); - p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim"); - p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim"); - p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim"); + p.reference_speed_points = + declare_parameter>("on_transition.reference_speed_points"); + p.lon_acc_lim = declare_parameter>("on_transition.lon_acc_lim"); + p.lon_jerk_lim = declare_parameter>("on_transition.lon_jerk_lim"); + p.lat_acc_lim = declare_parameter>("on_transition.lat_acc_lim"); + p.lat_jerk_lim = declare_parameter>("on_transition.lat_jerk_lim"); + p.actual_steer_diff_lim = + declare_parameter>("on_transition.actual_steer_diff_lim"); filter_on_transition_.setParam(p); } @@ -411,9 +423,11 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_; } - // Apply limit filtering - filtered_commands.control = filterControlCommand(filtered_commands.control); - + // Check if command filtering option is enable + if (enable_cmd_limit_filter_) { + // Apply limit filtering + filtered_commands.control = filterControlCommand(filtered_commands.control); + } // tmp: Publish vehicle emergency status VehicleEmergencyStamped vehicle_cmd_emergency; vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_); @@ -500,9 +514,14 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; + filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); + filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x); + + IsFilterActivated is_filter_activated; + // Apply transition_filter when transiting from MANUAL to AUTO. if (mode.is_in_transition) { - filter_on_transition_.filterAll(dt, current_steer_, out); + filter_on_transition_.filterAll(dt, current_steer_, out, is_filter_activated); } else { // When ego is stopped and the input command is not stopping, // use the higher of actual vehicle longitudinal state @@ -521,7 +540,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont : current_status_cmd.longitudinal.speed; filter_.setPrevCmd(prev_cmd); } - filter_.filterAll(dt, current_steer_, out); + filter_.filterAll(dt, current_steer_, out, is_filter_activated); } // set prev value for both to keep consistency over switching: @@ -544,6 +563,9 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont filter_.setPrevCmd(prev_values); filter_on_transition_.setPrevCmd(prev_values); + is_filter_activated.stamp = now(); + is_filter_activated_pub_->publish(is_filter_activated); + return out; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 17bfe99d45251..d516ecac29ca2 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -64,6 +65,7 @@ using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; +using vehicle_cmd_gate::msg::IsFilterActivated; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; @@ -99,6 +101,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr gate_mode_pub_; rclcpp::Publisher::SharedPtr engage_pub_; rclcpp::Publisher::SharedPtr operation_mode_pub_; + rclcpp::Publisher::SharedPtr is_filter_activated_pub_; // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; @@ -164,6 +167,7 @@ class VehicleCmdGate : public rclcpp::Node double stop_hold_acceleration_; double emergency_acceleration_; double moderate_stop_service_acceleration_; + bool enable_cmd_limit_filter_; // Service rclcpp::Service::SharedPtr srv_engage_; diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp new file mode 100644 index 0000000000000..6384fbfb22f60 --- /dev/null +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -0,0 +1,400 @@ +// 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. + +#include "../../src/vehicle_cmd_gate.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +#include + +#include +#include +#include +#include +#include + +#define ASSERT_LT_NEAR(x, y, alpha) ASSERT_LT(x, y * alpha) +#define ASSERT_GT_NEAR(x, y, alpha) ASSERT_GT(x, y * alpha) + +#define PRINT_VALUES(...) print_values(0, #__VA_ARGS__, __VA_ARGS__) +template +void print_values([[maybe_unused]] int i, [[maybe_unused]] T name) +{ + std::cerr << std::endl; +} +template +void print_values(int i, const T1 & name, const T2 & a, const T3 &... b) +{ + for (; name[i] != ',' && name[i] != '\0'; i++) std::cerr << name[i]; + + std::ostringstream oss; + oss << std::setprecision(4) << std::setw(9) << a; + std::cerr << ":" << oss.str() << " "; + print_values(i + 1, name, b...); +} + +// global params +const std::vector reference_speed_points = {5., 10., 15., 20.}; +const std::vector lon_acc_lim = {1.5, 1.0, 0.8, 0.6}; +const std::vector lon_jerk_lim = {1.4, 0.9, 0.7, 0.5}; +const std::vector lat_acc_lim = {2.0, 1.6, 1.2, 0.8}; +const std::vector lat_jerk_lim = {1.7, 1.3, 0.9, 0.6}; +const std::vector actual_steer_diff_lim = {0.5, 0.4, 0.2, 0.1}; +const double wheelbase = 2.89; + +using vehicle_cmd_gate::VehicleCmdGate; + +using autoware_adapi_v1_msgs::msg::MrmState; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; +using tier4_control_msgs::msg::GateMode; +using tier4_external_api_msgs::msg::Emergency; +using tier4_external_api_msgs::msg::Heartbeat; +using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; + +class PubSubNode : public rclcpp::Node +{ +public: + PubSubNode() : Node{"test_vehicle_cmd_gate_filter_pubsub"} + { + sub_cmd_ = create_subscription( + "output/control_cmd", rclcpp::QoS{1}, + [this](const AckermannControlCommand::ConstSharedPtr msg) { + cmd_history_.push_back(msg); + cmd_received_times_.push_back(now()); + checkFilter(); + }); + + rclcpp::QoS qos{1}; + qos.transient_local(); + + pub_external_emergency_stop_heartbeat_ = + create_publisher("input/external_emergency_stop_heartbeat", qos); + pub_engage_ = create_publisher("input/engage", qos); + pub_gate_mode_ = create_publisher("input/gate_mode", qos); + pub_odom_ = create_publisher("/localization/kinematic_state", qos); + pub_acc_ = create_publisher("input/acceleration", qos); + pub_steer_ = create_publisher("input/steering", qos); + pub_operation_mode_ = create_publisher("input/operation_mode", qos); + pub_mrm_state_ = create_publisher("input/mrm_state", qos); + + pub_auto_control_cmd_ = + create_publisher("input/auto/control_cmd", qos); + pub_auto_turn_indicator_cmd_ = + create_publisher("input/auto/turn_indicators_cmd", qos); + pub_auto_hazard_light_cmd_ = + create_publisher("input/auto/hazard_lights_cmd", qos); + pub_auto_gear_cmd_ = create_publisher("input/auto/gear_cmd", qos); + } + + rclcpp::Subscription::SharedPtr sub_cmd_; + + rclcpp::Publisher::SharedPtr pub_external_emergency_stop_heartbeat_; + rclcpp::Publisher::SharedPtr pub_engage_; + rclcpp::Publisher::SharedPtr pub_gate_mode_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_acc_; + rclcpp::Publisher::SharedPtr pub_steer_; + rclcpp::Publisher::SharedPtr pub_operation_mode_; + rclcpp::Publisher::SharedPtr pub_mrm_state_; + rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_turn_indicator_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_hazard_light_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_gear_cmd_; + + std::vector cmd_history_; + std::vector raw_cmd_history_; + std::vector cmd_received_times_; + + // publish except for the control_cmd + void publishDefaultTopicsNoSpin() + { + { + Heartbeat msg; + msg.stamp = now(); + pub_external_emergency_stop_heartbeat_->publish(msg); + } + { + EngageMsg msg; + msg.stamp = now(); + msg.engage = true; + pub_engage_->publish(msg); + } + { + GateMode msg; + msg.data = GateMode::AUTO; + pub_gate_mode_->publish(msg); + } + { + Odometry msg; // initialized for zero pose and twist + msg.header.frame_id = "baselink"; + msg.header.stamp = now(); + msg.pose.pose.orientation.w = 1.0; + msg.twist.twist.linear.x = 0.0; + if (!cmd_history_.empty()) { // ego moves as commanded. + msg.twist.twist.linear.x = + cmd_history_.back()->longitudinal.speed; // ego moves as commanded. + } else { + } + pub_odom_->publish(msg); + } + { + AccelWithCovarianceStamped msg; + msg.header.frame_id = "baselink"; + msg.header.stamp = now(); + msg.accel.accel.linear.x = 0.0; + if (!cmd_history_.empty()) { // ego moves as commanded. + msg.accel.accel.linear.x = cmd_history_.back()->longitudinal.acceleration; + } + pub_acc_->publish(msg); + } + { + SteeringReport msg; + msg.stamp = now(); + msg.steering_tire_angle = 0.0; + if (!cmd_history_.empty()) { // ego moves as commanded. + msg.steering_tire_angle = cmd_history_.back()->lateral.steering_tire_angle; + } + pub_steer_->publish(msg); + } + { + OperationModeState msg; + msg.stamp = now(); + msg.mode = OperationModeState::AUTONOMOUS; + msg.is_autoware_control_enabled = true; + pub_operation_mode_->publish(msg); + } + { + MrmState msg; + msg.stamp = now(); + msg.state = MrmState::NORMAL; + msg.behavior = MrmState::NONE; + pub_mrm_state_->publish(msg); + } + { + TurnIndicatorsCommand msg; + msg.stamp = now(); + msg.command = TurnIndicatorsCommand::DISABLE; + pub_auto_turn_indicator_cmd_->publish(msg); + } + { + HazardLightsCommand msg; + msg.stamp = now(); + msg.command = HazardLightsCommand::DISABLE; + pub_auto_hazard_light_cmd_->publish(msg); + } + { + GearCommand msg; + msg.stamp = now(); + msg.command = GearCommand::DRIVE; + pub_auto_gear_cmd_->publish(msg); + } + } + + void publishControlCommand(AckermannControlCommand msg) + { + msg.stamp = now(); + pub_auto_control_cmd_->publish(msg); + raw_cmd_history_.push_back(std::make_shared(msg)); + } + + void checkFilter() + { + if (cmd_history_.size() != cmd_received_times_.size()) { + throw std::logic_error("cmd history and received times must have same size. Check code."); + } + + if (cmd_history_.size() == 1) return; + + const size_t i_curr = cmd_history_.size() - 1; + const size_t i_prev = cmd_history_.size() - 2; + const auto cmd_curr = cmd_history_.at(i_curr); + const auto cmd_prev = cmd_history_.at(i_prev); + + const auto max_lon_acc_lim = *std::max_element(lon_acc_lim.begin(), lon_acc_lim.end()); + const auto max_lon_jerk_lim = *std::max_element(lon_jerk_lim.begin(), lon_jerk_lim.end()); + const auto max_lat_acc_lim = *std::max_element(lat_acc_lim.begin(), lat_acc_lim.end()); + const auto max_lat_jerk_lim = *std::max_element(lat_jerk_lim.begin(), lat_jerk_lim.end()); + + const auto dt = (cmd_received_times_.at(i_curr) - cmd_received_times_.at(i_prev)).seconds(); + const auto lon_vel = cmd_curr->longitudinal.speed; + const auto lon_acc = cmd_curr->longitudinal.acceleration; + const auto lon_jerk = (lon_acc - cmd_prev->longitudinal.acceleration) / dt; + const auto lat_acc = + lon_vel * lon_vel * std::tan(cmd_curr->lateral.steering_tire_angle) / wheelbase; + const auto prev_lon_vel = cmd_prev->longitudinal.speed; + const auto prev_lat_acc = + prev_lon_vel * prev_lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; + const auto lat_jerk = (lat_acc - prev_lat_acc) / dt; + + /* debug print */ + // const auto steer = cmd_curr->lateral.steering_tire_angle; + // PRINT_VALUES( + // dt, lon_vel, lon_acc, lon_jerk, lat_acc, lat_jerk, steer, max_lon_acc_lim, + // max_lon_jerk_lim, max_lat_acc_lim, max_lat_jerk_lim); + + // Output command must be smaller than maximum limit. + // TODO(Horibe): check for each velocity range. + constexpr auto threshold_scale = 1.1; + if (std::abs(lon_vel) > 0.01) { + ASSERT_LT_NEAR(std::abs(lon_acc), max_lon_acc_lim, threshold_scale); + ASSERT_LT_NEAR(std::abs(lon_jerk), max_lon_jerk_lim, threshold_scale); + ASSERT_LT_NEAR(std::abs(lat_acc), max_lat_acc_lim, threshold_scale); + ASSERT_LT_NEAR(std::abs(lat_jerk), max_lat_jerk_lim, threshold_scale); + } + } +}; + +struct CmdParam +{ + double max; + double freq; + double bias; + CmdParam() {} + CmdParam(double m, double f, double b) : max(m), freq(f), bias(b) {} +}; + +struct CmdParams +{ + CmdParam steering; + CmdParam velocity; + CmdParam acceleration; + CmdParams() {} + CmdParams(CmdParam s, CmdParam v, CmdParam a) : steering(s), velocity(v), acceleration(a) {} +}; + +class ControlCmdGenerator +{ +public: + CmdParams p_; // used for sin wave command generation + + using Clock = std::chrono::high_resolution_clock; + std::chrono::time_point start_time_{Clock::now()}; + + // generate ControlCommand with sin wave format. + // TODO(Horibe): implement steering_rate and jerk command. + AckermannControlCommand calcSinWaveCommand(bool reset_clock = false) + { + if (reset_clock) { + start_time_ = Clock::now(); + } + + const auto dt_ns = + std::chrono::duration_cast(Clock::now() - start_time_); + const auto dt_s = dt_ns.count() / 1e9; + + const auto sinWave = [&](auto amp, auto freq, auto bias) { + return amp * std::sin(2.0 * M_PI * freq * dt_s + bias); + }; + + AckermannControlCommand cmd; + cmd.lateral.steering_tire_angle = sinWave(p_.steering.max, p_.steering.freq, p_.steering.bias); + cmd.longitudinal.speed = + sinWave(p_.velocity.max, p_.velocity.freq, p_.velocity.bias) + p_.velocity.max; + cmd.longitudinal.acceleration = + sinWave(p_.acceleration.max, p_.acceleration.freq, p_.acceleration.bias); + + return cmd; + } +}; + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto vehicle_cmd_gate_dir = + ament_index_cpp::get_package_share_directory("vehicle_cmd_gate"); + const auto vehicle_info_util_dir = + ament_index_cpp::get_package_share_directory("vehicle_info_util"); + + node_options.arguments( + {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml", + "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"}); + + const auto override = [&](const auto s, const std::vector v) { + node_options.append_parameter_override>(s, v); + }; + + node_options.append_parameter_override("wheel_base", wheelbase); + override("nominal.reference_speed_points", reference_speed_points); + override("nominal.reference_speed_points", reference_speed_points); + override("nominal.lon_acc_lim", lon_acc_lim); + override("nominal.lon_jerk_lim", lon_jerk_lim); + override("nominal.lat_acc_lim", lat_acc_lim); + override("nominal.lat_jerk_lim", lat_jerk_lim); + override("nominal.actual_steer_diff_lim", actual_steer_diff_lim); + + return std::make_shared(node_options); +} + +class TestFixture : public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + vehicle_cmd_gate_node_ = generateNode(); + cmd_generator_.p_ = GetParam(); + } + + void TearDown() override + { + // rclcpp::shutdown(); + } + + PubSubNode pub_sub_node_; + std::shared_ptr vehicle_cmd_gate_node_; + ControlCmdGenerator cmd_generator_; +}; + +TEST_P(TestFixture, CheckFilterForSinCmd) +{ + [[maybe_unused]] auto a = std::system("ros2 node list"); + [[maybe_unused]] auto b = std::system("ros2 node info /test_vehicle_cmd_gate_filter_pubsub"); + [[maybe_unused]] auto c = std::system("ros2 node info /vehicle_cmd_gate"); + + for (size_t i = 0; i < 100; ++i) { + const bool reset_clock = (i == 0); + const auto cmd = cmd_generator_.calcSinWaveCommand(reset_clock); + pub_sub_node_.publishControlCommand(cmd); + pub_sub_node_.publishDefaultTopicsNoSpin(); + for (int i = 0; i < 20; ++i) { + rclcpp::spin_some(pub_sub_node_.get_node_base_interface()); + rclcpp::spin_some(vehicle_cmd_gate_node_->get_node_base_interface()); + } + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } + + std::cerr << "received cmd num = " << pub_sub_node_.cmd_received_times_.size() << std::endl; +}; + +// High frequency, large value +CmdParams p1 = {/*steer*/ {10, 1, 0}, /*velocity*/ {10, 1.2, 0}, /*acc*/ {5, 1.5, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam1, TestFixture, ::testing::Values(p1)); + +// High frequency, normal value +CmdParams p2 = {/*steer*/ {1.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam2, TestFixture, ::testing::Values(p2)); + +// High frequency, small value +CmdParams p3 = {/*steer*/ {1.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam3, TestFixture, ::testing::Values(p3)); + +// Low frequency +CmdParams p4 = {/*steer*/ {10, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +INSTANTIATE_TEST_SUITE_P(TestParam4, TestFixture, ::testing::Values(p4)); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 5fbfec237b047..7ce8580120652 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -25,19 +25,25 @@ #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) using autoware_auto_control_msgs::msg::AckermannControlCommand; +using vehicle_cmd_gate::LimitArray; constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( - vehicle_cmd_gate::VehicleCmdFilter & f, double v, double a, double j, double lat_a, double lat_j, - double wheelbase) + vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, + LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, const double wheelbase) { - f.setVelLim(v); - f.setLonAccLim(a); - f.setLonJerkLim(j); - f.setLatAccLim(lat_a); - f.setLatJerkLim(lat_j); - f.setWheelBase(wheelbase); + vehicle_cmd_gate::VehicleCmdFilterParam p; + p.vel_lim = v; + p.wheel_base = wheelbase; + p.reference_speed_points = speed_points; + p.lat_acc_lim = lat_a; + p.lat_jerk_lim = lat_j; + p.lon_acc_lim = a; + p.lon_jerk_lim = j; + p.actual_steer_diff_lim = steer_diff; + + f.setParam(p); } AckermannControlCommand genCmd(double s, double sr, double v, double a) @@ -56,15 +62,29 @@ double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } -void test_all( - double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, +double calcLatJerk( + const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, + const double wheelbase, const double dt) +{ + const auto prev_v = prev_cmd.longitudinal.speed; + const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; + + const auto curr_v = cmd.longitudinal.speed; + const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; + + return (curr - prev) / dt; +} + +void test_1d_limit( + double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, double STEER_DIFF, const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] vehicle_cmd_gate::VehicleCmdFilter filter; - setFilterParams(filter, V_LIM, A_LIM, J_LIM, LAT_A_LIM, LAT_J_LIM, WHEELBASE); + setFilterParams( + filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE); filter.setPrevCmd(prev_cmd); // velocity filter @@ -164,6 +184,23 @@ void test_all( filtered_cmd.lateral.steering_tire_angle, raw_cmd.lateral.steering_tire_angle, THRESHOLD); } } + + // steer diff + { + const auto current_steering = 0.1; + auto filtered_cmd = raw_cmd; + filter.limitActualSteerDiff(current_steering, filtered_cmd); + const auto filtered_steer_diff = filtered_cmd.lateral.steering_tire_angle - current_steering; + const auto raw_steer_diff = raw_cmd.lateral.steering_tire_angle - current_steering; + // check if the filtered value does not exceed the limit. + ASSERT_LT_NEAR(std::abs(filtered_steer_diff), STEER_DIFF); + + // check if the undesired filter is not applied. + if (std::abs(raw_steer_diff) < STEER_DIFF) { + ASSERT_NEAR( + filtered_cmd.lateral.steering_tire_angle, raw_cmd.lateral.steering_tire_angle, THRESHOLD); + } + } } TEST(VehicleCmdFilter, VehicleCmdFilter) @@ -173,6 +210,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector j_arr = {0.0, 0.1, 1.0}; const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; + const std::vector steer_diff_arr = {0.01, 1.0, 100.0}; const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; @@ -187,7 +225,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) for (const auto & lj : lat_j_arr) { for (const auto & prev_cmd : prev_cmd_arr) { for (const auto & raw_cmd : raw_cmd_arr) { - test_all(v, a, j, la, lj, prev_cmd, raw_cmd); + for (const auto & steer_diff : steer_diff_arr) { + test_1d_limit(v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + } } } } @@ -196,3 +236,228 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) } } } + +TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) +{ + constexpr double WHEELBASE = 2.8; + vehicle_cmd_gate::VehicleCmdFilter filter; + + vehicle_cmd_gate::VehicleCmdFilterParam p; + p.wheel_base = WHEELBASE; + p.vel_lim = 20.0; + p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + p.lon_acc_lim = std::vector{0.3, 0.4, 0.5}; + p.lon_jerk_lim = std::vector{0.4, 0.4, 0.7}; + p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; + p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; + p.actual_steer_diff_lim = std::vector{0.1, 0.3, 0.2}; + filter.setParam(p); + + const auto DT = 0.033; + + const auto orig_cmd = []() { + AckermannControlCommand cmd; + cmd.lateral.steering_tire_angle = 0.5; + cmd.lateral.steering_tire_rotation_rate = 0.5; + cmd.longitudinal.speed = 30.0; + cmd.longitudinal.acceleration = 10.0; + cmd.longitudinal.jerk = 10.0; + return cmd; + }(); + + const auto set_speed_and_reset_prev = [&](const auto & current_vel) { + filter.setCurrentSpeed(current_vel); + }; + + const auto _limitLongitudinalWithVel = [&](const auto & in) { + auto out = in; + filter.limitLongitudinalWithVel(out); + return out; + }; + const auto _limitLongitudinalWithAcc = [&](const auto & in) { + auto out = in; + filter.limitLongitudinalWithAcc(DT, out); + return out; + }; + const auto _limitLongitudinalWithJerk = [&](const auto & in) { + auto out = in; + filter.limitLongitudinalWithJerk(DT, out); + return out; + }; + const auto _limitLateralWithLatAcc = [&](const auto & in) { + auto out = in; + filter.limitLateralWithLatAcc(DT, out); + return out; + }; + const auto _limitLateralWithLatJerk = [&](const auto & in) { + auto out = in; + filter.limitLateralWithLatJerk(DT, out); + return out; + }; + const auto _limitActualSteerDiff = [&](const auto & in) { + auto out = in; + const auto prev_steering = 0.0; + filter.limitActualSteerDiff(prev_steering, out); + return out; + }; + + constexpr double ep = 1.0e-5; + + // vel lim + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); + } + + // longitudinal acc lim + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.3, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.3, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.35, ep); + + set_speed_and_reset_prev(5.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.4 + 0.1 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.4 + 0.4 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.5, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_limitLongitudinalWithAcc(orig_cmd).longitudinal.acceleration, 0.5, ep); + } + + // longitudinal jerk lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.lon_jerk_lim = std::vector{0.4, 0.4, 0.7}; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.4, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.4, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.4, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.4, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.4, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.4, ep); + + set_speed_and_reset_prev(5.0); + const auto expect_v5 = 0.4 + 0.3 / 6.0; + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, expect_v5, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * expect_v5, ep); + + set_speed_and_reset_prev(8.0); + const auto expect_v8 = 0.4 + 1.2 / 6.0; + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, expect_v8, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * expect_v8, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.7, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.7, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.jerk, 0.7, ep); + EXPECT_NEAR(_limitLongitudinalWithJerk(orig_cmd).longitudinal.acceleration, DT * 0.7, ep); + } + + // lateral acc lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; + const auto _calcLatAcc = [&](const auto & cmd) { return calcLatAcc(cmd, WHEELBASE); }; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.15, ep); + + set_speed_and_reset_prev(5.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.1 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.4 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + } + + // lateral jerk lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; + const auto _calcLatJerk = [&](const auto & cmd) { + return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT); + }; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.8, ep); + + set_speed_and_reset_prev(5.0); + const auto expect_v5 = 0.7 - 0.6 * (1.0 / 6.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v5, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v5, ep); + + set_speed_and_reset_prev(8.0); + const auto expect_v8 = 0.7 - 0.6 * (4.0 / 6.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v8, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + } + + // steering diff lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.actual_steer_diff_lim = std::vector{0.1, 0.3, 0.2}; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.1, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.1, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.2, ep); + + set_speed_and_reset_prev(5.0); + const auto expect_v5 = 0.3 - 0.1 / 6.0; + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), expect_v5, ep); + + set_speed_and_reset_prev(8.0); + const auto expect_v8 = 0.3 - 0.4 / 6.0; + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), expect_v8, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.2, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR((_limitActualSteerDiff(orig_cmd).lateral.steering_tire_angle), 0.2, ep); + } +} diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index b9b86e6bd2d12..4d97757c3c33e 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -64,10 +64,10 @@ Stat calcTimeToCollision( const double dt = traj_dist / std::abs(p0.longitudinal_velocity_mps); t += dt; for (auto obstacle : obstacles.objects) { - const double obst_dist = + const double obstacle_dist = calcDistance2d(p, obstacle.kinematics.initial_pose_with_covariance.pose); // TODO(Maxime CLEMENT): take shape into consideration - if (obst_dist <= distance_threshold) { + if (obstacle_dist <= distance_threshold) { stat.add(t); break; } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 8cb9c726fcb05..116055dd9e87e 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -49,6 +49,8 @@ def launch_setup(context, *args, **kwargs): vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("lane_departure_checker_param_path").perform(context), "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("control_validator_param_path").perform(context), "r") as f: + control_validator_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" ) as f: @@ -112,6 +114,23 @@ def launch_setup(context, *args, **kwargs): parameters=[nearest_search_param, lane_departure_checker_param, vehicle_info_param], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # control validator checker + control_validator_component = ComposableNode( + package="control_validator", + plugin="control_validator::ControlValidator", + name="control_validator", + remappings=[ + ("~/input/kinematics", "/localization/kinematic_state"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ("~/output/validation_status", "~/validation_status"), + ], + parameters=[control_validator_param], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) # shift decider shift_decider_component = ComposableNode( @@ -287,6 +306,12 @@ def launch_setup(context, *args, **kwargs): target_container="/control/control_container", ) + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + # set container to run all required components in the same process container = ComposableNodeContainer( name="control_container", @@ -295,10 +320,12 @@ def launch_setup(context, *args, **kwargs): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ controller_component, + control_validator_component, lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, operation_mode_transition_manager_component, + glog_component, ], ) @@ -338,6 +365,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("lon_controller_param_path") add_launch_arg("vehicle_cmd_gate_param_path") add_launch_arg("lane_departure_checker_param_path") + add_launch_arg("control_validator_param_path") add_launch_arg("operation_mode_transition_manager_param_path") add_launch_arg("shift_decider_param_path") add_launch_arg("obstacle_collision_checker_param_path") diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 550c9b5ff7fff..8538179253480 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -14,7 +14,7 @@ - + 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 new file mode 100644 index 0000000000000..4de9aec81bb38 --- /dev/null +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + 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 0cbda728b7fdc..abbb15c797d19 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 @@ -112,4 +112,27 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml index ebee784510435..a9b0b93947054 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml @@ -21,33 +21,43 @@ - - - - + - - - - - + + + + + - - - - - - - - - + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py index ef9f25e43cf90..1a34429f438ed 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ b/launch/tier4_localization_launch/launch/util/util.launch.py @@ -34,7 +34,7 @@ def load_composable_node_param(param_path): plugin="pointcloud_preprocessor::CropBoxFilterComponent", name="crop_box_filter_measurement_range", remappings=[ - ("input", LaunchConfiguration("input/pointcloud")), + ("input", LaunchConfiguration("input_pointcloud")), ("output", "measurement_range/pointcloud"), ], parameters=[ diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 93af4ee68a6f7..f00752d1c14fc 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + ar_tag_based_localizer automatic_pose_initializer eagleye_fix2pose eagleye_gnss_converter @@ -26,6 +27,7 @@ topic_tools yabloc_common yabloc_image_processing + yabloc_monitor yabloc_particle_filter yabloc_pose_initializer diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index f9459be3b8924..23bd2fc337c97 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -12,13 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os + +from ament_index_python import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition +from launch.launch_description_sources import AnyLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import Node @@ -57,7 +62,9 @@ def launch_setup(context, *args, **kwargs): package="map_loader", plugin="Lanelet2MapLoaderNode", name="lanelet2_map_loader", - remappings=[("output/lanelet2_map", "vector_map")], + remappings=[ + ("output/lanelet2_map", "vector_map"), + ], parameters=[ { "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), @@ -110,6 +117,19 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + map_projection_loader_launch_file = os.path.join( + get_package_share_directory("map_projection_loader"), + "launch", + "map_projection_loader.launch.xml", + ) + map_projection_loader = IncludeLaunchDescription( + AnyLaunchDescriptionSource(map_projection_loader_launch_file), + launch_arguments={ + "map_projector_info_path": LaunchConfiguration("map_projector_info_path"), + "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), + }.items(), + ) + container = ComposableNodeContainer( name="map_container", namespace="", @@ -129,6 +149,7 @@ def launch_setup(context, *args, **kwargs): PushRosNamespace("map"), container, map_hash_generator, + map_projection_loader, ] ) @@ -159,6 +180,11 @@ def add_launch_arg(name: str, default_value=None, description=None): [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], "path to pointcloud map metadata file", ), + add_launch_arg( + "map_projector_info_path", + [LaunchConfiguration("map_path"), "/map_projector_info.yaml"], + "path to map projector info yaml file", + ), add_launch_arg( "lanelet2_map_loader_param_path", [ diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 6f75950838267..d8f69c124526a 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -13,6 +13,7 @@ autoware_cmake map_loader + map_projection_loader map_tf_generator ament_lint_auto diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 2d983dc47545c..79e04e1d6fc61 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -224,17 +224,56 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + @@ -255,6 +294,7 @@ + @@ -262,11 +302,19 @@ - + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index bfb30e860f070..5b6134dc45584 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -11,6 +11,13 @@ + + + + + + + @@ -105,7 +112,7 @@ - + @@ -115,6 +122,13 @@ + + + + + + + @@ -134,6 +148,7 @@ + @@ -141,11 +156,19 @@ - + + + + + + + + + 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 dc3e33faafb94..a5d8d2113d2db 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 @@ -65,7 +65,7 @@ def create_normal_pipeline(self): ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { @@ -122,7 +122,7 @@ def create_down_sample_pipeline(self): ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { 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 0f2712a03247e..751eeea66c7b6 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 @@ -12,6 +12,8 @@ + + @@ -42,6 +44,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 204fb561b10ea..6634c36ff3768 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -22,6 +22,7 @@ + @@ -67,6 +68,8 @@ /> + + 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 4dae46cbacb0a..1f86a89ddfc7a 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 @@ -9,6 +9,7 @@ + @@ -27,7 +28,7 @@ - + @@ -77,7 +78,7 @@ - + @@ -130,7 +131,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml index c1ace97b9788f..ec7545956e774 100644 --- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -1,7 +1,18 @@ - - - + + + + + + + + + + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index fc1fac04b9415..59e8038e49188 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -64,6 +64,12 @@ def launch_setup(context, *args, **kwargs): with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f: behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + behavior_path_planner_component = ComposableNode( package="behavior_path_planner", plugin="behavior_path_planner::BehaviorPathPlannerNode", @@ -102,9 +108,6 @@ def launch_setup(context, *args, **kwargs): behavior_path_planner_param, vehicle_param, { - "lane_change.cancel.enable_on_lane_changing_phase": LaunchConfiguration( - "use_experimental_lane_change_function" - ), "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration( "use_experimental_lane_change_function" ), @@ -214,6 +217,7 @@ def launch_setup(context, *args, **kwargs): composable_node_descriptions=[ behavior_path_planner_component, behavior_velocity_planner_component, + glog_component, ], output="screen", ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 34ad4659b5ece..d8c5d7825f19d 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -309,6 +309,16 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), ) + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + glog_component_loader = LoadComposableNodes( + composable_node_descriptions=[glog_component], + target_container=container, + ) + group = GroupAction( [ container, @@ -319,6 +329,7 @@ def launch_setup(context, *args, **kwargs): obstacle_cruise_planner_loader, obstacle_cruise_planner_relay_loader, surround_obstacle_checker_loader, + glog_component_loader, ] ) return [group] 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 665bd82423ea6..b293c5836817d 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 @@ -24,15 +24,25 @@ - - - - - - - - - + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 4de0f91c53c6a..39b95286bb6cc 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -60,6 +60,7 @@ external_cmd_selector external_velocity_limit_selector freespace_planner + glog_component mission_planner motion_velocity_smoother obstacle_avoidance_planner diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 6f83ca0793798..aef1e45f517b1 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -16,7 +16,8 @@ - + + diff --git a/localization/ar_tag_based_localizer/CMakeLists.txt b/localization/ar_tag_based_localizer/CMakeLists.txt new file mode 100644 index 0000000000000..89337b9bb966c --- /dev/null +++ b/localization/ar_tag_based_localizer/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.14) +project(ar_tag_based_localizer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +# +# ar_tag_based_localizer +# +ament_auto_add_executable(ar_tag_based_localizer + src/ar_tag_based_localizer_node.cpp + src/ar_tag_based_localizer_core.cpp + src/utils.cpp +) +target_include_directories(ar_tag_based_localizer + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} +) +target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) + +# +# tag_tf_caster +# +ament_auto_add_executable(tag_tf_caster + src/tag_tf_caster_node.cpp + src/tag_tf_caster_core.cpp +) +target_include_directories(tag_tf_caster + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/ar_tag_based_localizer/README.md b/localization/ar_tag_based_localizer/README.md new file mode 100644 index 0000000000000..e471378773cbb --- /dev/null +++ b/localization/ar_tag_based_localizer/README.md @@ -0,0 +1,154 @@ +# AR Tag Based Localizer + +**ArTagBasedLocalizer** is a vision-based localization package. + + + +This package 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. + +This package includes two nodes. + +- `ar_tag_based_localizer` : A node that detects AR-Tags from camera images and publishes the pose of the ego vehicle. +- `tag_tf_caster` : A node that publishes the pose of the AR-Tags applied in Lanelet2 as `tf_static`. + +## Inputs / Outputs + +### `ar_tag_based_localizer` node + +#### Input + +| Name | Type | Description | +| :-------------------- | :----------------------------- | :----------- | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | +| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | +| `tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | + +### `tag_tf_caster` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | + +#### Output + +| Name | Type | Description | +| :---------- | :------------------------------------- | :----------------- | +| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | + +## How to launch + +When launching Autoware, specify `artag` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=artag \ + ... +``` + +[Sample rosbag and map](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) + +This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. + +The image below shows the trajectory when the sample is executed and plotted. + +![sample_result](./doc_image/sample_result.png) + +The pull request video below should also be helpful. + + + +## Architecture + +![node diagram](./doc_image/node_diagram.drawio.svg) + +## Principle + +### `ar_tag_based_localizer` node + +![principle](./doc_image/principle.png) + +### `tag_tf_caster` node + +The definitions of the tags written to the map are introduced in the next section. See `Map Specifications`. + +The `tag_tf_caster` node publishes the TF from the map to the tag. + +- Translation : The center of the four vertices of the tag +- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. + +Users can define tags as Lanelet2 4-vertex polygons. +In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the tag in that case is difficult to calculate. +So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the tag will not publish tf_static. + +## Map specifications + +For this package to function correctly, the pose of the AR-Tags must be specified in the Lanelet2 map format that Autoware can interpret. + +The four vertices of AR-Tag are defined counterclockwise. + +The order of the four vertices is defined as follows. In the coordinate system of AR-Tag, + +- 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 AR-Tag is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. + +```xml +... + + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + +... + +``` diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml new file mode 100644 index 0000000000000..385e86498c58c --- /dev/null +++ b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + # marker_size + marker_size: 0.6 + + # target_tag_ids + target_tag_ids: ['0','4','5'] + + # covariance + covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + # Detect AR-Tags within this range and publish the pose of ego vehicle + distance_threshold: 6.0 # [m] + + # Camera frame id + camera_frame: "camera" + + # Detector parameters + # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 + detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] + min_marker_size: 0.02 diff --git a/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml b/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml new file mode 100644 index 0000000000000..2fe2869038e82 --- /dev/null +++ b/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + # Tags with a volume greater than this value will not cast their pose + volume_threshold: 1e-5 # [m^3] diff --git a/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png new file mode 100644 index 0000000000000..9d2aa204ba24e Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png differ diff --git a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg new file mode 100644 index 0000000000000..bc8ba297558bb --- /dev/null +++ b/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg @@ -0,0 +1,156 @@ + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + y + + + + + + + + + + +
+
+
+ z +
+
+
+ + z + + + + + + +
+
+
+ AR Tag(Front) +
+
+
+ + AR Tag(Front) + + + + + + + Text is not SVG - cannot display + + + diff --git a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg b/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg new file mode 100644 index 0000000000000..a339951663b29 --- /dev/null +++ b/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg @@ -0,0 +1,86 @@ + + + + + + + AR Tag Based + Localizer + + + + Other Autoware + packages + + + + + + /twist_with_covariance + + + + + + /image + + + + + + /pose_with_covariance + + + + /ar_tag_based_localizer + + + + /ekf_localizer + + + + + + /camera_info + + + + + + + Lanelet2 + + + + + + /tf_static + + + + /tag_tf_caster + + + + + + /lanelet2_map_loader + + + diff --git a/localization/ar_tag_based_localizer/doc_image/principle.png b/localization/ar_tag_based_localizer/doc_image/principle.png new file mode 100644 index 0000000000000..4dc6cfb7624e6 Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/principle.png differ diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result.png b/localization/ar_tag_based_localizer/doc_image/sample_result.png new file mode 100644 index 0000000000000..3ced77f592f5d Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/sample_result.png differ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp new file mode 100644 index 0000000000000..39373d92add25 --- /dev/null +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -0,0 +1,103 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''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 Rafael Muñoz Salinas 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. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#ifndef AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ +#define AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +class ArTagBasedLocalizer : public rclcpp::Node +{ +public: + ArTagBasedLocalizer(); + bool setup(); + +private: + void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); + void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); + void publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg); + + // Parameters + float marker_size_{}; + std::vector target_tag_ids_; + std::vector covariance_; + double distance_threshold_squared_{}; + std::string camera_frame_; + + // tf + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + + // image transport + std::unique_ptr it_; + + // Subscribers + image_transport::Subscriber image_sub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + + // Publishers + image_transport::Publisher image_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; + + // Others + aruco::MarkerDetector detector_; + aruco::CameraParameters cam_param_; + bool cam_info_received_; +}; + +#endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp new file mode 100644 index 0000000000000..5b10fe5b1dc0f --- /dev/null +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#define AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ + +#include + +#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" +#include + +#include +#include +#include + +#include + +class TagTfCaster : public rclcpp::Node +{ +public: + TagTfCaster(); + +private: + void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); + void publish_tf(const lanelet::Polygon3d & poly); + + // Parameters + double volume_threshold_; + + // tf + std::unique_ptr tf_buffer_; + std::unique_ptr tf_broadcaster_; + + // Subscribers + rclcpp::Subscription::SharedPtr map_bin_sub_; +}; + +#endif // AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp new file mode 100644 index 0000000000000..06401dad8fdd1 --- /dev/null +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''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 Rafael Muñoz Salinas 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. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#ifndef AR_TAG_BASED_LOCALIZER__UTILS_HPP_ +#define AR_TAG_BASED_LOCALIZER__UTILS_HPP_ + +#include + +#include +#include + +/** + * @brief ros_camera_info_to_aruco_cam_params gets the camera intrinsics from a CameraInfo message + * and copies them to aruco's data structure + * @param cam_info + * @param use_rectified_parameters if true, the intrinsics are taken from cam_info.P and the + * distortion parameters are set to 0. Otherwise, cam_info.K and cam_info.D are taken. + * @return + */ +aruco::CameraParameters ros_camera_info_to_aruco_cam_params( + const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters); + +tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); + +#endif // AR_TAG_BASED_LOCALIZER__UTILS_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml new file mode 100644 index 0000000000000..940f6ed53d8b8 --- /dev/null +++ b/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml new file mode 100644 index 0000000000000..6a9c3dcd17e2e --- /dev/null +++ b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/ar_tag_based_localizer/package.xml b/localization/ar_tag_based_localizer/package.xml new file mode 100644 index 0000000000000..4527d2ad9b086 --- /dev/null +++ b/localization/ar_tag_based_localizer/package.xml @@ -0,0 +1,30 @@ + + + + ar_tag_based_localizer + 0.0.0 + The ar_tag_based_localizer package + Shintaro Sakoda + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake + autoware_cmake + + aruco + autoware_auto_mapping_msgs + cv_bridge + geometry_msgs + image_transport + lanelet2_extension + rclcpp + sensor_msgs + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs + + + ament_cmake + + diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp new file mode 100644 index 0000000000000..ca3bfe2e4a6b0 --- /dev/null +++ b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -0,0 +1,272 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''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 Rafael Muñoz Salinas 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. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" + +#include "ar_tag_based_localizer/utils.hpp" + +#include +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +ArTagBasedLocalizer::ArTagBasedLocalizer() +: Node("ar_tag_based_localizer"), cam_info_received_(false) +{ +} + +bool ArTagBasedLocalizer::setup() +{ + /* + Declare node parameters + */ + marker_size_ = static_cast(this->declare_parameter("marker_size")); + target_tag_ids_ = this->declare_parameter>("target_tag_ids"); + covariance_ = this->declare_parameter>("covariance"); + distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); + camera_frame_ = this->declare_parameter("camera_frame"); + std::string detection_mode = this->declare_parameter("detection_mode"); + float min_marker_size = static_cast(this->declare_parameter("min_marker_size")); + if (detection_mode == "DM_NORMAL") { + detector_.setDetectionMode(aruco::DM_NORMAL, min_marker_size); + } else if (detection_mode == "DM_FAST") { + detector_.setDetectionMode(aruco::DM_FAST, min_marker_size); + } else if (detection_mode == "DM_VIDEO_FAST") { + detector_.setDetectionMode(aruco::DM_VIDEO_FAST, min_marker_size); + } else { + // Error + RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); + return false; + } + + /* + Log parameter info + */ + RCLCPP_INFO_STREAM(this->get_logger(), "min_marker_size: " << min_marker_size); + RCLCPP_INFO_STREAM(this->get_logger(), "detection_mode: " << detection_mode); + RCLCPP_INFO_STREAM(this->get_logger(), "thresMethod: " << detector_.getParameters().thresMethod); + RCLCPP_INFO_STREAM(this->get_logger(), "marker_size_: " << marker_size_); + + /* + tf + */ + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + tf_broadcaster_ = std::make_unique(this); + + /* + Initialize image transport + */ + it_ = std::make_unique(shared_from_this()); + + /* + Subscribers + */ + image_sub_ = it_->subscribe("~/input/image", 1, &ArTagBasedLocalizer::image_callback, this); + cam_info_sub_ = this->create_subscription( + "~/input/camera_info", 1, + std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + + /* + Publishers + */ + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos.reliable(); + qos.transient_local(); + image_pub_ = it_->advertise("~/debug/result", 1); + pose_pub_ = this->create_publisher( + "~/output/pose_with_covariance", qos); + + RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); + return true; +} + +void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) +{ + if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { + RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); + return; + } + + if (!cam_info_received_) { + RCLCPP_DEBUG(this->get_logger(), "No cam_info has been received."); + return; + } + + builtin_interfaces::msg::Time curr_stamp = msg->header.stamp; + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + cv::Mat in_image = cv_ptr->image; + + // detection results will go into "markers" + std::vector markers; + + // ok, let's detect + detector_.detect(in_image, markers, cam_param_, marker_size_, false); + + // for each marker, draw info and its boundaries in the image + for (const aruco::Marker & marker : markers) { + tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker); + + geometry_msgs::msg::TransformStamped tf_cam_to_marker_stamped; + tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); + tf_cam_to_marker_stamped.header.stamp = curr_stamp; + tf_cam_to_marker_stamped.header.frame_id = camera_frame_; + tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); + tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); + + geometry_msgs::msg::PoseStamped pose_cam_to_marker; + tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); + pose_cam_to_marker.header.stamp = curr_stamp; + pose_cam_to_marker.header.frame_id = std::to_string(marker.id); + publish_pose_as_base_link(pose_cam_to_marker); + + // drawing the detected markers + marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + } + + // draw a 3d cube in each marker if there is 3d info + if (cam_param_.isValid()) { + for (aruco::Marker & marker : markers) { + aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); + } + } + + if (image_pub_.getNumSubscribers() > 0) { + // show input with augmented information + cv_bridge::CvImage out_msg; + out_msg.header.stamp = curr_stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = in_image; + image_pub_.publish(out_msg.toImageMsg()); + } +} + +// wait for one camera info, then shut down that subscriber +void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & msg) +{ + if (cam_info_received_) { + return; + } + + cam_param_ = ros_camera_info_to_aruco_cam_params(msg, true); + cam_info_received_ = true; +} + +void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg) +{ + // Check if frame_id is in target_tag_ids + if ( + std::find(target_tag_ids_.begin(), target_tag_ids_.end(), msg.header.frame_id) == + target_tag_ids_.end()) { + RCLCPP_INFO_STREAM( + this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in target_tag_ids"); + return; + } + + // Range filter + const double distance_squared = msg.pose.position.x * msg.pose.position.x + + msg.pose.position.y * msg.pose.position.y + + msg.pose.position.z * msg.pose.position.z; + if (distance_threshold_squared_ < distance_squared) { + return; + } + + // Transform map to tag + geometry_msgs::msg::TransformStamped map_to_tag_tf; + try { + map_to_tag_tf = + tf_buffer_->lookupTransform("map", "tag_" + msg.header.frame_id, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform map to tag_%s: %s", msg.header.frame_id.c_str(), + ex.what()); + return; + } + + // Transform camera to base_link + geometry_msgs::msg::TransformStamped camera_to_base_link_tf; + try { + camera_to_base_link_tf = + tf_buffer_->lookupTransform(camera_frame_, "base_link", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); + return; + } + + // Convert ROS Transforms to Eigen matrices for easy matrix multiplication and inversion + Eigen::Affine3d map_to_tag = tf2::transformToEigen(map_to_tag_tf.transform); + Eigen::Affine3d camera_to_base_link = tf2::transformToEigen(camera_to_base_link_tf.transform); + Eigen::Affine3d camera_to_tag = Eigen::Affine3d( + Eigen::Translation3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) * + Eigen::Quaterniond( + msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z)); + Eigen::Affine3d tag_to_camera = camera_to_tag.inverse(); + + // Final pose calculation + Eigen::Affine3d map_to_base_link = map_to_tag * tag_to_camera * camera_to_base_link; + + // Construct output message + geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance_stamped; + pose_with_covariance_stamped.header.stamp = msg.header.stamp; + pose_with_covariance_stamped.header.frame_id = "map"; + pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link); + std::copy( + covariance_.begin(), covariance_.end(), pose_with_covariance_stamped.pose.covariance.begin()); + + pose_pub_->publish(pose_with_covariance_stamped); +} diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp new file mode 100644 index 0000000000000..92d2e35ee3c1b --- /dev/null +++ b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp @@ -0,0 +1,54 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''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 Rafael Muñoz Salinas 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. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" + +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/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp new file mode 100644 index 0000000000000..d0279caf88dda --- /dev/null +++ b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp @@ -0,0 +1,125 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +TagTfCaster::TagTfCaster() : Node("tag_tf_caster") +{ + // Parameters + volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5); + + // tf + tf_buffer_ = std::make_unique(this->get_clock()); + tf_broadcaster_ = std::make_unique(this); + + // Subscribers + map_bin_sub_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + std::bind(&TagTfCaster::map_bin_callback, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "finish setup"); +} + +void TagTfCaster::map_bin_callback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) +{ + RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version); + RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_format: " << msg->map_format); + RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_version: " << msg->map_version); + RCLCPP_INFO_STREAM(this->get_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; + } + publish_tf(poly); + } +} + +void TagTfCaster::publish_tf(const lanelet::Polygon3d & poly) +{ + // Get marker_id + const std::string marker_id = poly.attributeOr("marker_id", "none"); + + // Get 4 vertices + const auto & vertices = poly.basicPolygon(); + if (vertices.size() != 4) { + RCLCPP_WARN_STREAM(this->get_logger(), "vertices.size() (" << vertices.size() << ") != 4"); + return; + } + + // 4 vertices represent the marker vertices in counterclockwise order + // Calculate the volume by considering it as a tetrahedron + const auto & v0 = vertices[0]; + const auto & v1 = vertices[1]; + 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(this->get_logger(), "volume = " << volume); + if (volume > volume_threshold_) { + RCLCPP_WARN_STREAM( + this->get_logger(), + "volume (" << volume << ") > threshold (" << volume_threshold_ << "), This is not plane."); + return; + } + + // Calculate the center of the quadrilateral + const auto center = (v0 + v1 + v2 + v3) / 4.0; + + // Define axes + const auto x_axis = (v1 - v0).normalized(); + const auto y_axis = (v2 - v1).normalized(); + const auto z_axis = x_axis.cross(y_axis).normalized(); + + // Construct rotation matrix and convert it to quaternion + Eigen::Matrix3d rotation_matrix; + rotation_matrix << x_axis, y_axis, z_axis; + const Eigen::Quaterniond q{rotation_matrix}; + + // Create transform + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = this->now(); + tf.header.frame_id = "map"; + tf.child_frame_id = "tag_" + marker_id; + tf.transform.translation.x = center.x(); + tf.transform.translation.y = center.y(); + tf.transform.translation.z = center.z(); + tf.transform.rotation.x = q.x(); + tf.transform.rotation.y = q.y(); + tf.transform.rotation.z = q.z(); + tf.transform.rotation.w = q.w(); + + // Publish transform + tf_broadcaster_->sendTransform(tf); + RCLCPP_INFO_STREAM(this->get_logger(), "id: " << marker_id); + RCLCPP_INFO_STREAM( + this->get_logger(), "(x, y, z) = " << tf.transform.translation.x << ", " + << tf.transform.translation.y << ", " + << tf.transform.translation.z); + RCLCPP_INFO_STREAM( + this->get_logger(), "q = " << tf.transform.rotation.x << ", " << tf.transform.rotation.y << ", " + << tf.transform.rotation.z << ", " << tf.transform.rotation.w); +} diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp b/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp new file mode 100644 index 0000000000000..ce83c572d6a6e --- /dev/null +++ b/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp @@ -0,0 +1,22 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/localization/ar_tag_based_localizer/src/utils.cpp b/localization/ar_tag_based_localizer/src/utils.cpp new file mode 100644 index 0000000000000..9f582830280d9 --- /dev/null +++ b/localization/ar_tag_based_localizer/src/utils.cpp @@ -0,0 +1,116 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This source code is derived from the https://github.com/pal-robotics/aruco_ros. +// Here is the license statement. +/***************************** + Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, are + permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + + THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''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 Rafael Muñoz Salinas 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. + + The views and conclusions contained in the software and documentation are those of the + authors and should not be interpreted as representing official policies, either expressed + or implied, of Rafael Muñoz Salinas. + ********************************/ + +#include "ar_tag_based_localizer/utils.hpp" + +#include +#include + +aruco::CameraParameters ros_camera_info_to_aruco_cam_params( + const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters) +{ + cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); + cv::Mat distortion_coeff(4, 1, CV_64FC1); + cv::Size size(static_cast(cam_info.width), static_cast(cam_info.height)); + + if (use_rectified_parameters) { + camera_matrix.setTo(0); + camera_matrix.at(0, 0) = cam_info.p[0]; + camera_matrix.at(0, 1) = cam_info.p[1]; + camera_matrix.at(0, 2) = cam_info.p[2]; + camera_matrix.at(0, 3) = cam_info.p[3]; + camera_matrix.at(1, 0) = cam_info.p[4]; + camera_matrix.at(1, 1) = cam_info.p[5]; + camera_matrix.at(1, 2) = cam_info.p[6]; + camera_matrix.at(1, 3) = cam_info.p[7]; + camera_matrix.at(2, 0) = cam_info.p[8]; + camera_matrix.at(2, 1) = cam_info.p[9]; + camera_matrix.at(2, 2) = cam_info.p[10]; + camera_matrix.at(2, 3) = cam_info.p[11]; + + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = 0; + } + } else { + cv::Mat camera_matrix_from_k(3, 3, CV_64FC1, 0.0); + for (int i = 0; i < 9; ++i) { + camera_matrix_from_k.at(i % 3, i - (i % 3) * 3) = cam_info.k[i]; + } + camera_matrix_from_k.copyTo(camera_matrix(cv::Rect(0, 0, 3, 3))); + + if (cam_info.d.size() == 4) { + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = cam_info.d[i]; + } + } else { + RCLCPP_WARN( + rclcpp::get_logger("ar_tag_based_localizer"), + "length of camera_info D vector is not 4, assuming zero distortion..."); + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = 0; + } + } + } + + return {camera_matrix, distortion_coeff, size}; +} + +tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker) +{ + cv::Mat rot(3, 3, CV_64FC1); + cv::Mat r_vec64; + marker.Rvec.convertTo(r_vec64, CV_64FC1); + cv::Rodrigues(r_vec64, rot); + cv::Mat tran64; + marker.Tvec.convertTo(tran64, CV_64FC1); + + tf2::Matrix3x3 tf_rot( + rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), + rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), + rot.at(2, 2)); + + tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + + return tf2::Transform(tf_rot, tf_orig); +} diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index bdcc7ac486cd0..748b5ee5becc0 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -18,6 +18,7 @@ This package includes the following features: - **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy. - **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored. - **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value, especially for low-frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see the following figure). +- **Calculation of vertical correction amount from pitch** mitigates localization instability on slopes. For example, when going uphill, it behaves as if it is buried in the ground (see the left side of the "Calculate delta from pitch" figure) because EKF only considers 3DoF(x,y,yaw). Therefore, EKF corrects the z-coordinate according to the formula (see the right side of the "Calculate delta from pitch" figure).

@@ -27,6 +28,10 @@ This package includes the following features:

+

+ +

+ ## Launch The `ekf_localizer` starts with the default parameters with the following command. diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 322325d239004..4d3f5b9643462 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: show_debug_info: false - enable_yaw_bias_estimation: True + enable_yaw_bias_estimation: true predict_frequency: 50.0 tf_rate: 50.0 extend_state_step: 50 diff --git a/localization/ekf_localizer/media/calculation_delta_from_pitch.png b/localization/ekf_localizer/media/calculation_delta_from_pitch.png new file mode 100644 index 0000000000000..0fa459f96dd71 Binary files /dev/null and b/localization/ekf_localizer/media/calculation_delta_from_pitch.png differ diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 8e837920f6d6a..5bc9c5e42712d 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -8,6 +8,7 @@ Koji Minoda Yamato Ando Takeshi Ishita + Masahiro Sakamoto 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 651eca1e9de12..0e46a26add852 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -446,7 +446,15 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); ekf_.updateWithDelay(y, C, R, delay_step); - updateSimple1DFilters(pose, params_.pose_smoothing_steps); + + // Considering change of z value due to measurement pose delay + const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const double dz_delay = current_ekf_twist_.twist.linear.x * delay_time * std::sin(-rpy.y); + geometry_msgs::msg::PoseWithCovarianceStamped pose_with_z_delay; + pose_with_z_delay = pose; + pose_with_z_delay.pose.pose.position.z += dz_delay; + + updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); // debug const Eigen::MatrixXd X_result = ekf_.getLatestX(); diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 26781513ff2f7..cbcaadebddc2e 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -5,6 +5,7 @@ 0.1.0 The gyro_odometer package as a ROS 2 node Yamato Ando + Masahiro Sakamoto Apache License 2.0 Yamato Ando diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 8ad9e77453db5..2412fd5d57602 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -102,7 +102,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) : Node("gyro_odometer", options), - output_frame_(declare_parameter("base_link", "base_link")), + output_frame_(declare_parameter("output_frame", "base_link")), message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)), vehicle_twist_arrived_(false), imu_arrived_(false) diff --git a/localization/initial_pose_button_panel/CMakeLists.txt b/localization/initial_pose_button_panel/CMakeLists.txt deleted file mode 100644 index 6c1c13e000a51..0000000000000 --- a/localization/initial_pose_button_panel/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(initial_pose_button_panel) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Qt5 REQUIRED Core Widgets) -set(QT_LIBRARIES Qt5::Widgets) - -add_definitions(-DQT_NO_KEYWORDS -g) -set(CMAKE_AUTOMOC ON) - -ament_auto_add_library(initial_pose_button_panel SHARED - src/initial_pose_button_panel.cpp) -target_link_libraries(initial_pose_button_panel - ${QT_LIBRARIES}) - -# Export the plugin to be imported by rviz2 -pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) - -ament_auto_package( - INSTALL_TO_SHARE - plugins -) diff --git a/localization/initial_pose_button_panel/README.md b/localization/initial_pose_button_panel/README.md deleted file mode 100644 index cdb4824ada0e2..0000000000000 --- a/localization/initial_pose_button_panel/README.md +++ /dev/null @@ -1,18 +0,0 @@ -# initial_pose_button_panel - -## Role - -`initial_pose_button_panel` is the package to send a request to the localization module to calculate the current ego pose. - -It starts calculating the current ego pose by pushing the button on Rviz, implemented as an Rviz plugin. -You can see the button on the right bottom of Rviz. - -![initialize_button](./media/initialize_button.png) - -## Input / Output - -### Input topics - -| Name | Type | Description | -| ---------------------------------------------- | --------------------------------------------- | -------------------------------------------------------------- | -| `/sensing/gnss/pose_with_covariance` (default) | geometry_msgs::msg::PoseWithCovarianceStamped | initial pose with covariance to calculate the current ego pose | diff --git a/localization/initial_pose_button_panel/media/initialize_button.png b/localization/initial_pose_button_panel/media/initialize_button.png deleted file mode 100644 index f7bfe8aa652ac..0000000000000 Binary files a/localization/initial_pose_button_panel/media/initialize_button.png and /dev/null differ diff --git a/localization/initial_pose_button_panel/package.xml b/localization/initial_pose_button_panel/package.xml deleted file mode 100644 index 2db87fe645fb1..0000000000000 --- a/localization/initial_pose_button_panel/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - initial_pose_button_panel - 0.1.0 - The initial_pose_button_panel package - Yamato ANDO - Apache License 2.0 - - Yamato ANDO - - ament_cmake_auto - autoware_cmake - - geometry_msgs - libqt5-core - libqt5-widgets - qtbase5-dev - rviz_common - tier4_localization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - - diff --git a/localization/initial_pose_button_panel/plugins/plugin_description.xml b/localization/initial_pose_button_panel/plugins/plugin_description.xml deleted file mode 100644 index eea08b60d03e1..0000000000000 --- a/localization/initial_pose_button_panel/plugins/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - initial button. - - - diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp deleted file mode 100644 index 654095641c7f4..0000000000000 --- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "initial_pose_button_panel.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace tier4_localization_rviz_plugin -{ -InitialPoseButtonPanel::InitialPoseButtonPanel(QWidget * parent) : rviz_common::Panel(parent) -{ - topic_label_ = new QLabel("PoseWithCovarianceStamped "); - topic_label_->setAlignment(Qt::AlignCenter); - - topic_edit_ = new QLineEdit("/sensing/gnss/pose_with_covariance"); - connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic())); - - initialize_button_ = new QPushButton("Wait for subscribe topic"); - initialize_button_->setEnabled(false); - connect(initialize_button_, SIGNAL(clicked(bool)), SLOT(pushInitializeButton())); - - status_label_ = new QLabel("Not Initialize"); - status_label_->setAlignment(Qt::AlignCenter); - status_label_->setStyleSheet("QLabel { background-color : gray;}"); - - QSizePolicy q_size_policy(QSizePolicy::Expanding, QSizePolicy::Expanding); - initialize_button_->setSizePolicy(q_size_policy); - - auto * topic_layout = new QHBoxLayout; - topic_layout->addWidget(topic_label_); - topic_layout->addWidget(topic_edit_); - - auto * v_layout = new QVBoxLayout; - v_layout->addLayout(topic_layout); - v_layout->addWidget(initialize_button_); - v_layout->addWidget(status_label_); - - setLayout(v_layout); -} -void InitialPoseButtonPanel::onInitialize() -{ - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - pose_cov_sub_ = raw_node->create_subscription( - topic_edit_->text().toStdString(), 10, - std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); - - client_ = raw_node->create_client( - "/localization/initialize"); -} - -void InitialPoseButtonPanel::callbackPoseCov( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) -{ - pose_cov_msg_ = *msg; - initialize_button_->setText("Pose Initializer Let's GO!"); - initialize_button_->setEnabled(true); -} - -void InitialPoseButtonPanel::editTopic() -{ - pose_cov_sub_.reset(); - rclcpp::Node::SharedPtr raw_node = - this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - pose_cov_sub_ = raw_node->create_subscription( - topic_edit_->text().toStdString(), 10, - std::bind(&InitialPoseButtonPanel::callbackPoseCov, this, std::placeholders::_1)); - initialize_button_->setText("Wait for subscribe topic"); - initialize_button_->setEnabled(false); -} - -void InitialPoseButtonPanel::pushInitializeButton() -{ - // lock button - initialize_button_->setEnabled(false); - - status_label_->setStyleSheet("QLabel { background-color : dodgerblue;}"); - status_label_->setText("Initializing..."); - - std::thread thread([this] { - auto req = std::make_shared(); - req->pose_with_covariance = pose_cov_msg_; - - client_->async_send_request( - req, [this]([[maybe_unused]] rclcpp::Client< - tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedFuture result) { - status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); - status_label_->setText("OK!!!"); - - // unlock button - initialize_button_->setEnabled(true); - }); - }); - - thread.detach(); -} - -} // end namespace tier4_localization_rviz_plugin - -PLUGINLIB_EXPORT_CLASS(tier4_localization_rviz_plugin::InitialPoseButtonPanel, rviz_common::Panel) diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp deleted file mode 100644 index 44defe637b7df..0000000000000 --- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.hpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#ifndef INITIAL_POSE_BUTTON_PANEL_HPP_ -#define INITIAL_POSE_BUTTON_PANEL_HPP_ - -#include -#include -#include -#include - -#include -#ifndef Q_MOC_RUN - -#include -#include -#include -#endif -#include -#include - -namespace tier4_localization_rviz_plugin -{ -class InitialPoseButtonPanel : public rviz_common::Panel -{ - Q_OBJECT - -public: - explicit InitialPoseButtonPanel(QWidget * parent = nullptr); - void onInitialize() override; - void callbackPoseCov(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); - -public Q_SLOTS: - void editTopic(); - void pushInitializeButton(); - -protected: - rclcpp::Subscription::SharedPtr pose_cov_sub_; - - rclcpp::Client::SharedPtr client_; - - QLabel * topic_label_; - QLineEdit * topic_edit_; - QPushButton * initialize_button_; - QLabel * status_label_; - - geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_msg_; -}; - -} // end namespace tier4_localization_rviz_plugin - -#endif // INITIAL_POSE_BUTTON_PANEL_HPP_ diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 4c563f0b30ab1..23e71f2a844cd 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -5,6 +5,7 @@ 0.1.0 ros node for monitoring localization error Yamato Ando + Masahiro Sakamoto Apache License 2.0 Taichi Higashide 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 index 5253879a28937..79454e89b9ed0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -39,7 +39,7 @@ class MapModule rclcpp::CallbackGroup::SharedPtr map_callback_group); private: - void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr); + void callback_map_points(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr); rclcpp::Subscription::SharedPtr map_points_sub_; std::shared_ptr ndt_ptr_; 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 61034e02e9c91..4f630bb8c5898 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 @@ -63,7 +63,7 @@ class MapUpdateModule const std::vector & maps_to_add, const std::vector & map_ids_to_remove); void update_map(const geometry_msgs::msg::Point & position); - bool should_update_map(const geometry_msgs::msg::Point & position) const; + [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position) const; void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp index e88862781225d..f1e7dfb3f544b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp @@ -30,18 +30,18 @@ class PoseArrayInterpolator public: PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters); PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array); PoseWithCovarianceStamped get_current_pose(); PoseWithCovarianceStamped get_old_pose(); PoseWithCovarianceStamped get_new_pose(); - bool is_success(); + [[nodiscard]] bool is_success() const; private: rclcpp::Logger logger_; @@ -51,10 +51,10 @@ class PoseArrayInterpolator PoseWithCovarianceStamped::SharedPtr new_pose_ptr_; bool success_; - bool validate_time_stamp_difference( + [[nodiscard]] bool validate_time_stamp_difference( const rclcpp::Time & target_time, const rclcpp::Time & reference_time, const double time_tolerance_sec) const; - bool validate_position_difference( + [[nodiscard]] bool validate_position_difference( const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 17db6b17d4a45..92c690a708492 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -7,6 +7,7 @@ Yamato Ando Kento Yabuuchi Koji Minoda + Masahiro Sakamoto Apache License 2.0 Yamato Ando diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp index cec62c850372a..9c82e06d89b80 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -30,7 +30,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale = scale; - marker.id = i; + marker.id = static_cast(i); marker.lifetime = rclcpp::Duration::from_seconds(10.0); // 10.0 is the lifetime in seconds. marker.ns = "initial_pose_transform_probability_color_marker"; @@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.ns = "initial_pose_index_color_marker"; marker.pose = particle.initial_pose; - marker.color = exchange_color_crc((1.0 * i) / 100); + marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); marker.ns = "result_pose_transform_probability_color_marker"; @@ -60,7 +60,7 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.ns = "result_pose_index_color_marker"; marker.pose = particle.result_pose; - marker.color = exchange_color_crc((1.0 * i) / 100); + marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); return marker_array; diff --git a/localization/ndt_scan_matcher/src/map_module.cpp b/localization/ndt_scan_matcher/src/map_module.cpp index e036f8f81d482..d6088a1b14949 100644 --- a/localization/ndt_scan_matcher/src/map_module.cpp +++ b/localization/ndt_scan_matcher/src/map_module.cpp @@ -18,7 +18,7 @@ MapModule::MapModule( rclcpp::Node * node, std::mutex * ndt_ptr_mutex, std::shared_ptr ndt_ptr, rclcpp::CallbackGroup::SharedPtr map_callback_group) -: ndt_ptr_(ndt_ptr), ndt_ptr_mutex_(ndt_ptr_mutex) +: 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; diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 26c2fd30d9198..9c94467dc94c0 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -28,13 +28,13 @@ MapUpdateModule::MapUpdateModule( std::shared_ptr tf2_listener_module, std::string map_frame, rclcpp::CallbackGroup::SharedPtr main_callback_group, std::shared_ptr> state_ptr) -: ndt_ptr_(ndt_ptr), +: ndt_ptr_(std::move(ndt_ptr)), ndt_ptr_mutex_(ndt_ptr_mutex), - map_frame_(map_frame), + map_frame_(std::move(map_frame)), logger_(node->get_logger()), clock_(node->get_clock()), - tf2_listener_module_(tf2_listener_module), - state_ptr_(state_ptr), + tf2_listener_module_(std::move(tf2_listener_module)), + state_ptr_(std::move(state_ptr)), dynamic_map_loading_update_distance_( node->declare_parameter("dynamic_map_loading_update_distance")), dynamic_map_loading_map_radius_( @@ -112,9 +112,9 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) { auto request = std::make_shared(); - request->area.center_x = position.x; - request->area.center_y = position.y; - request->area.radius = dynamic_map_loading_map_radius_; + request->area.center_x = static_cast(position.x); + request->area.center_y = static_cast(position.y); + request->area.radius = static_cast(dynamic_map_loading_map_radius_); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); // // send a request to map_loader @@ -163,9 +163,9 @@ void MapUpdateModule::update_ndt( backup_ndt.createVoxelKdtree(); const auto exe_end_time = std::chrono::system_clock::now(); - const double exe_time = - std::chrono::duration_cast(exe_end_time - exe_start_time).count() / - 1000.0; + const auto duration_micro_sec = + std::chrono::duration_cast(exe_end_time - exe_start_time).count(); + 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 diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp index d7635c052142f..59ecedc18cb5a 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp @@ -15,7 +15,7 @@ #include "ndt_scan_matcher/pose_array_interpolator.hpp" PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array) : logger_(node->get_logger()), clock_(*node->get_clock()), @@ -33,7 +33,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( } PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time target_ros_time, + rclcpp::Node * node, const rclcpp::Time & target_ros_time, const std::deque & pose_msg_ptr_array, const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) : PoseArrayInterpolator(node, target_ros_time, pose_msg_ptr_array) @@ -50,7 +50,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( pose_distance_tolerance_meters); // all validations must be true - if (!(is_old_pose_valid & is_new_pose_valid & is_pose_diff_valid)) { + if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { RCLCPP_WARN(logger_, "Validation error."); } } @@ -70,7 +70,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pos return *new_pose_ptr_; } -bool PoseArrayInterpolator::is_success() +bool PoseArrayInterpolator::is_success() const { return success_; } diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index 2db6b51540d51..d947c56dfe183 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -28,19 +28,19 @@ std_msgs::msg::ColorRGBA exchange_color_crc(double x) if (x <= 0.25) { color.b = 1.0; - color.g = std::sin(x * 2.0 * M_PI); + color.g = static_cast(std::sin(x * 2.0 * M_PI)); color.r = 0; } else if (x > 0.25 && x <= 0.5) { - color.b = std::sin(x * 2 * M_PI); + color.b = static_cast(std::sin(x * 2 * M_PI)); color.g = 1.0; color.r = 0; } else if (x > 0.5 && x <= 0.75) { color.b = 0; color.g = 1.0; - color.r = -std::sin(x * 2.0 * M_PI); + color.r = static_cast(-std::sin(x * 2.0 * M_PI)); } else { color.b = 0; - color.g = -std::sin(x * 2.0 * M_PI); + color.g = static_cast(-std::sin(x * 2.0 * M_PI)); color.r = 1.0; } color.a = 0.999; @@ -58,9 +58,9 @@ double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) return diff_rad; } -Eigen::Map makeEigenCovariance(const std::array & covariance) +Eigen::Map make_eigen_covariance(const std::array & covariance) { - return Eigen::Map(covariance.data(), 6, 6); + return {covariance.data(), 6, 6}; } // x: roll, y: pitch, z: yaw @@ -242,7 +242,7 @@ std::vector create_random_pose_array( { std::default_random_engine engine(seed_gen()); const Eigen::Map covariance = - makeEigenCovariance(base_pose_with_cov.pose.covariance); + make_eigen_covariance(base_pose_with_cov.pose.covariance); std::normal_distribution<> x_distribution(0.0, std::sqrt(covariance(0, 0))); std::normal_distribution<> y_distribution(0.0, std::sqrt(covariance(1, 1))); diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index ed044bd285909..83bce5a718334 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -5,6 +5,7 @@ 0.1.0 The pose2twist package Yamato Ando + Masahiro Sakamoto Apache License 2.0 Yamato Ando diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 5b7713d7c4afd..43e8d48c4fda0 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index cb519c49e6335..f8333d89fa8ac 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -6,6 +6,7 @@ The pose_initializer package Yamato Ando Takagi, Isamu + Masahiro Sakamoto Apache License 2.0 Yamato Ando Takagi, Isamu diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index b0beeb4758951..b17809aaa9948 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -6,6 +6,7 @@ The stop filter package Koji Minoda Yamato Ando + Masahiro Sakamoto Apache License 2.0 Koji Minoda diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index dc54311b3a1db..c44cd9d144566 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -6,6 +6,7 @@ The acceleration estimation package Koji Minoda Yamato Ando + Masahiro Sakamoto Apache License 2.0 Koji Minoda diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index d56b111df45d3..1c3ba4de307f6 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -4,6 +4,7 @@ 0.1.0 YabLoc common library Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index bf9a0abc08f8e..150ffed58138d 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -1,9 +1,9 @@ - + - - + + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 92d8210be85a3..961ce574f7c7a 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -5,6 +5,7 @@ 0.0.0 YabLoc image processing package Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 6a127c70bb9a0..57f2302fd5c5a 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -54,7 +54,9 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st { tier4_autoware_utils::StopWatch stop_watch; line_segment_detector_->detect(gray_image, lines); - line_segment_detector_->drawSegments(gray_image, lines); + if (lines.size().width != 0) { + line_segment_detector_->drawSegments(gray_image, lines); + } RCLCPP_INFO_STREAM(this->get_logger(), "lsd: " << stop_watch.toc() << "[ms]"); } diff --git a/localization/yabloc/yabloc_monitor/CMakeLists.txt b/localization/yabloc/yabloc_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..aa1515661b2f6 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(yabloc_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(yabloc_monitor + src/yabloc_monitor_node.cpp + src/yabloc_monitor_core.cpp + src/availability_module.cpp +) +ament_target_dependencies(yabloc_monitor) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/yabloc/yabloc_monitor/README.md b/localization/yabloc/yabloc_monitor/README.md new file mode 100644 index 0000000000000..ed4cdc36b6ba0 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/README.md @@ -0,0 +1,27 @@ +# yabloc_monitor + +YabLoc monitor is a node that monitors the status of the YabLoc localization system. It is a wrapper node that monitors the status of the YabLoc localization system and publishes the status as diagnostics. + +## Feature + +### Availability + +The node monitors the final output pose of YabLoc to verify the availability of YabLoc. + +### Others + +To be added, + +## Interfaces + +### Input + +| Name | Type | Description | +| --------------------- | --------------------------- | ------------------------------- | +| `~/input/yabloc_pose` | `geometry_msgs/PoseStamped` | The final output pose of YabLoc | + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | diff --git a/localization/yabloc/yabloc_monitor/config/yabloc_monitor.param.yaml b/localization/yabloc/yabloc_monitor/config/yabloc_monitor.param.yaml new file mode 100644 index 0000000000000..b0a7ce9d9a6b8 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/config/yabloc_monitor.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + availability/timestamp_tolerance: 1.0 diff --git a/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml b/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml new file mode 100644 index 0000000000000..cf9f73977d35d --- /dev/null +++ b/localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml new file mode 100644 index 0000000000000..8c92c3c6a4303 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -0,0 +1,26 @@ + + + + yabloc_monitor + 0.1.0 + YabLoc monitor package + Kento Yabuuchi + Koji Minoda + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake_ros + autoware_cmake + + diagnostic_updater + geometry_msgs + rclcpp + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/yabloc/yabloc_monitor/src/availability_module.cpp b/localization/yabloc/yabloc_monitor/src/availability_module.cpp new file mode 100644 index 0000000000000..5933b12d15016 --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/availability_module.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 TIER IV +// +// 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 "availability_module.hpp" + +#include + +#include + +AvailabilityModule::AvailabilityModule(rclcpp::Node * node) +: clock_(node->get_clock()), + latest_yabloc_pose_stamp_(std::nullopt), + timestamp_threshold_(node->declare_parameter("availability/timestamp_tolerance")) +{ + sub_yabloc_pose_ = node->create_subscription( + "~/input/yabloc_pose", 10, + [this](const PoseStamped::ConstSharedPtr msg) { on_yabloc_pose(msg); }); +} + +bool AvailabilityModule::is_available() const +{ + if (!latest_yabloc_pose_stamp_.has_value()) { + return false; + } + + const auto now = clock_->now(); + + const auto diff_time = now - latest_yabloc_pose_stamp_.value(); + const auto diff_time_sec = diff_time.seconds(); + return diff_time_sec < timestamp_threshold_; +} + +void AvailabilityModule::on_yabloc_pose(const PoseStamped::ConstSharedPtr msg) +{ + latest_yabloc_pose_stamp_ = rclcpp::Time(msg->header.stamp); +} diff --git a/localization/yabloc/yabloc_monitor/src/availability_module.hpp b/localization/yabloc/yabloc_monitor/src/availability_module.hpp new file mode 100644 index 0000000000000..40d0ea335493c --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/availability_module.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 TIER IV +// +// 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 AVAILABILITY_MODULE_HPP_ +#define AVAILABILITY_MODULE_HPP_ + +#include + +#include + +#include +#include + +class AvailabilityModule +{ +private: + using PoseStamped = geometry_msgs::msg::PoseStamped; + +public: + explicit AvailabilityModule(rclcpp::Node * node); + [[nodiscard]] bool is_available() const; + +private: + rclcpp::Subscription::SharedPtr sub_yabloc_pose_; + void on_yabloc_pose(PoseStamped::ConstSharedPtr msg); + + rclcpp::Clock::SharedPtr clock_; + std::optional latest_yabloc_pose_stamp_; + const double timestamp_threshold_; +}; + +#endif // AVAILABILITY_MODULE_HPP_ diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp new file mode 100644 index 0000000000000..876b86fd2bc9e --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp @@ -0,0 +1,48 @@ +// Copyright 2023 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "yabloc_monitor_core.hpp" + +#include + +#include + +YabLocMonitor::YabLocMonitor() : Node("yabloc_monitor"), updater_(this) +{ + updater_.setHardwareID(get_name()); + updater_.add("yabloc_status", this, &YabLocMonitor::update_diagnostics); + + // Set timer + using std::chrono_literals::operator""ms; + timer_ = create_wall_timer(100ms, [this] { on_timer(); }); + // Evaluation modules + availability_module_ = std::make_unique(this); +} + +void YabLocMonitor::on_timer() +{ + updater_.force_update(); +} + +void YabLocMonitor::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + bool is_available = availability_module_->is_available(); + stat.add("Availability", is_available ? "OK" : "NG"); + + if (is_available) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "NG"); + } +} diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp new file mode 100644 index 0000000000000..8b8b937da205b --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp @@ -0,0 +1,40 @@ +// Copyright 2023 TIER IV +// +// 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 YABLOC_MONITOR_CORE_HPP_ +#define YABLOC_MONITOR_CORE_HPP_ + +#include "availability_module.hpp" + +#include +#include + +#include + +class YabLocMonitor : public rclcpp::Node +{ +public: + YabLocMonitor(); + +private: + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void on_timer(); + + diagnostic_updater::Updater updater_; + rclcpp::TimerBase::SharedPtr timer_; + + // Evaluation modules + std::unique_ptr availability_module_; +}; +#endif // YABLOC_MONITOR_CORE_HPP_ diff --git a/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp new file mode 100644 index 0000000000000..9b58325c852ea --- /dev/null +++ b/localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "yabloc_monitor_core.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + auto node = std::make_shared(); + + rclcpp::spin(node); + + return 0; +} diff --git a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt index 5f56186077d48..1284c52690795 100644 --- a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt +++ b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt @@ -49,7 +49,6 @@ ament_auto_add_library( ) target_include_directories(abstract_corrector SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) target_link_libraries(abstract_corrector Sophus::Sophus ${PCL_LIBRARIES}) -rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") # Prediction library ament_auto_add_library(predictor @@ -60,7 +59,16 @@ ament_auto_add_library(predictor ) target_include_directories(predictor SYSTEM PRIVATE ${PCL_INCLUDE_DIRS}) target_link_libraries(predictor Sophus::Sophus ${PCL_LIBRARIES}) -rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# ros2idl typesupport +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(abstract_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp") + rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(abstract_corrector "${cpp_typesupport_target}") + target_link_libraries(predictor "${cpp_typesupport_target}") +endif() # Cost map Library ament_auto_add_library(ll2_cost_map 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 60bb501fd1ae7..f38264b828d34 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 @@ -9,7 +9,7 @@ - + @@ -19,9 +19,9 @@ - - - + + + @@ -47,7 +47,7 @@ - + diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 7a70b8ad6a8fb..6d11b12d9c922 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -5,6 +5,7 @@ 0.1.0 YabLoc particle filter package Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_ros diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index a2aaba44b6626..cd54a3c5f2ef2 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -44,9 +44,16 @@ ament_auto_add_executable(${TARGET} src/camera/camera_pose_initializer_node.cpp) target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) +# ros2idl typesupport +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${TARGET} "${cpp_typesupport_target}") +endif() + # Semantic segmentation install(PROGRAMS src/semantic_segmentation/semantic_segmentation_core.py diff --git a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml index 1f40fb619b0b7..a1d5d59b46094 100644 --- a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml +++ b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml @@ -1,17 +1,17 @@ - - + + - - + + - + diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index 9adfc2a85f663..f6e8aa83bafc7 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -4,6 +4,7 @@ 0.0.0 The pose initializer Kento Yabuuchi + Masahiro Sakamoto Apache License 2.0 ament_cmake_ros diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index b288da6aca2cb..8f6ccf969c9a4 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -6,6 +6,7 @@ The map_height_fitter package Takagi, Isamu Yamato Ando + Masahiro Sakamoto Apache License 2.0 Takagi, Isamu diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 0494b57dcfab9..816cfc418f016 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -20,11 +20,12 @@ Currently, it supports the following two types: 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: -1. **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. -2. **The division size along each axis should be equal.** -3. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). -4. **All the split maps should not overlap with each other.** -5. **Metadata file should also be provided.** The metadata structure description is provided below. +1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). +2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. +3. **The division size along each axis should be equal.** +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). +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). @@ -69,6 +70,7 @@ sample-map-rosbag │ ├── B.pcd │ ├── C.pcd │ └── ... +├── map_projector_info.yaml └── pointcloud_map_metadata.yaml ``` @@ -138,27 +140,39 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. -The node projects lan/lon coordinates into MGRS coordinates. +The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. +The node supports the following three types of coordinate systems: + +- MGRS +- LocalCartesianUTM +- local ### How to run `ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` +### Subscribed Topics + +- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware + ### Published Topics - ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +### Parameters + +| Name | Type | Description | Default value | +| :--------------------- | :---------- | :----------------------------------------------- | :------------ | +| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | +| lanelet2_map_path | std::string | The lanelet2 map path | None | + --- ## lanelet2_map_visualization ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. There are 3 types of map can be loaded in autoware. Please make sure you selected the correct lanelet2_map_projector_type when you launch this package. - -- MGRS -- UTM -- local +lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -171,13 +185,3 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Published Topics - ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz - -### Parameters - -| Name | Type | Description | Default value | -| :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ | -| lanelet2_map_projector_type | std::string | The type of the map projector using, can be MGRS, UTM, local | MGRS | -| latitude | double | Latitude of map_origin, only using in UTM map projector | 0.0 | -| longitude | double | Longitude of map_origin, only using in UTM map projector | 0.0 | -| center_line_resolution | double | Define the resolution of the lanelet center line | 5.0 | -| lanelet2_map_path | std::string | The lanelet2 map path | None | diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 971af0147b0fe..24d2b0e8ed7a8 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,7 +1,3 @@ /**: ros__parameters: - lanelet2_map_projector_type: MGRS # Options: MGRS, UTM, local - latitude: 40.816617984672746 # Latitude of map_origin, using in UTM - longitude: 29.360491808334285 # Longitude of map_origin, using in UTM - center_line_resolution: 5.0 # [m] diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 16681f3bd290d..d2e9f66ad047a 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -15,6 +15,8 @@ #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#include +#include #include #include @@ -36,16 +38,17 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0); - static const MapProjectorInfo get_map_projector_type( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon); static HADMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + + void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Publisher::SharedPtr pub_map_bin_; - rclcpp::Publisher::SharedPtr pub_map_projector_type_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index e9ed6af24edd9..fbc2572d0bc6d 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -7,6 +7,7 @@ Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda + Masahiro Sakamoto Apache License 2.0 @@ -15,6 +16,8 @@ autoware_auto_mapping_msgs autoware_map_msgs + component_interface_specs + component_interface_utils fmt geometry_msgs lanelet2_extension 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 aafa66a35be84..c443318a146ac 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 @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -50,15 +51,26 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) : Node("lanelet2_map_loader", options) { - const auto lanelet2_filename = declare_parameter("lanelet2_map_path", ""); - const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS"); - const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0); - const double map_origin_lat = declare_parameter("latitude", 0.0); - const double map_origin_lon = declare_parameter("longitude", 0.0); + const auto adaptor = component_interface_utils::NodeAdaptor(this); + + // subscription + adaptor.init_sub( + sub_map_projector_info_, + [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); + + declare_parameter("lanelet2_map_path", ""); + declare_parameter("center_line_resolution", 5.0); +} + +void Lanelet2MapLoaderNode::on_map_projector_info( + const MapProjectorInfo::Message::ConstSharedPtr msg) +{ + const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); + const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); // load map from file const auto map = - load_map(lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon); + load_map(lanelet2_filename, msg->type, msg->map_origin.latitude, msg->map_origin.longitude); if (!map) { return; } @@ -69,16 +81,10 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options // create map bin msg const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now()); - const auto map_projector_type_msg = get_map_projector_type( - lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon); // create publisher and publish pub_map_bin_ = create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); - // create publisher and publish - pub_map_projector_type_ = - create_publisher("map_projector_type", rclcpp::QoS{1}.transient_local()); - pub_map_projector_type_->publish(map_projector_type_msg); } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( @@ -92,11 +98,10 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( if (errors.empty()) { return map; } - } else if (lanelet2_map_projector_type == "UTM") { + } else if (lanelet2_map_projector_type == "LocalCartesianUTM") { lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); if (errors.empty()) { return map; @@ -137,27 +142,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -const MapProjectorInfo Lanelet2MapLoaderNode::get_map_projector_type( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type, - const double & map_origin_lat, const double & map_origin_lon) -{ - lanelet::ErrorMessages errors{}; - MapProjectorInfo map_projector_type_msg; - if (lanelet2_map_projector_type == "MGRS") { - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - map_projector_type_msg.type = "MGRS"; - map_projector_type_msg.mgrs_grid = projector.getProjectedMGRSGrid(); - } else if (lanelet2_map_projector_type == "UTM") { - map_projector_type_msg.type = "UTM"; - map_projector_type_msg.map_origin.latitude = map_origin_lat; - map_projector_type_msg.map_origin.longitude = map_origin_lon; - } else { - map_projector_type_msg.type = "local"; - } - return map_projector_type_msg; -} - HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { 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 df845ca9bd252..53794fb93b7c6 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 @@ -125,13 +125,15 @@ void Lanelet2MapVisualizationNode::onMapBin( viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); lanelet::ConstPolygons3d hatched_road_markings_area = lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); + std::vector no_parking_reg_elems = + lanelet::utils::query::noParkingAreas(all_lanelets); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line; + cl_hatched_road_markings_line, cl_no_parking_areas; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -153,6 +155,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); + setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; @@ -235,6 +238,10 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + pub_marker_->publish(map_marker_array); } diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt new file mode 100644 index 0000000000000..8658ba172fde2 --- /dev/null +++ b/map/map_projection_loader/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.14) +project(map_projection_loader) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_find_build_dependencies() + +ament_auto_add_library(map_projection_loader_lib SHARED + src/map_projection_loader.cpp + src/load_info_from_lanelet2_map.cpp +) + +target_link_libraries(map_projection_loader_lib yaml-cpp) + +ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) + +target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) + +target_link_libraries(map_projection_loader map_projection_loader_lib) +target_include_directories(map_projection_loader PUBLIC include) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" map_projection_loader_lib) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +if(BUILD_TESTING) + # Test python scripts + add_launch_test( + test/test_node_load_mgrs_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_local_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_local_cartesian_utm_from_yaml.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) + + # Test c++ scripts + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_load_info_from_lanelet2_map.cpp) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md new file mode 100644 index 0000000000000..e31db7890cf47 --- /dev/null +++ b/map/map_projection_loader/README.md @@ -0,0 +1,62 @@ +# map_projection_loader + +## Feature + +`map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate Autoware is operating. +This is necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. + +- If `map_projector_info_path` DOES exist, this node loads it and publishes the map projection information accordingly. +- If `map_projector_info_path` does NOT exist, the node assumes that you are using the `MGRS` projection type, and loads the lanelet2 map instead to extract the MGRS grid. + - **DEPRECATED WARNING: This interface that uses the lanelet2 map is not recommended. Please prepare the YAML file instead.** + +## Map projector info file specification + +You need to provide a YAML file, namely `map_projector_info.yaml` under the `map_path` directory. For `pointcloud_map_metadata.yaml`, please refer to the Readme of `map_loader`. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +├── map_projector_info.yaml +└── pointcloud_map_metadata.yaml +``` + +### Using local coordinate + +```yaml +# map_projector_info.yaml +type: "local" +``` + +### Using MGRS + +If you want to use MGRS, please specify the MGRS grid as well. + +```yaml +# map_projector_info.yaml +type: "MGRS" +mgrs_grid: "54SUE" +``` + +### Using LocalCartesianUTM + +If you want to use local cartesian UTM, please specify the map origin as well. + +```yaml +# map_projector_info.yaml +type: "LocalCartesianUTM" +map_origin: + latitude: 35.6092 + longitude: 139.7303 +``` + +## Published Topics + +- ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information + +## Parameters + +| Name | Type | Description | +| :---------------------- | :---------- | :------------------------------------------------------------------------------- | +| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default) | +| lanelet2_map_path | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) | diff --git a/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp new file mode 100644 index 0000000000000..746f16e0f6b33 --- /dev/null +++ b/map/map_projection_loader/include/map_projection_loader/load_info_from_lanelet2_map.hpp @@ -0,0 +1,29 @@ +// 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 MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ +#define MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ + +#include +#include +#include +#include + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); + +#endif // MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ 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 new file mode 100644 index 0000000000000..16442eb43c740 --- /dev/null +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -0,0 +1,37 @@ +// 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 MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ +#define MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); + +class MapProjectionLoader : public rclcpp::Node +{ +public: + MapProjectionLoader(); + +private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + component_interface_utils::Publisher::SharedPtr publisher_; +}; + +#endif // MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml new file mode 100644 index 0000000000000..fc625e3162911 --- /dev/null +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml new file mode 100644 index 0000000000000..823cea064515b --- /dev/null +++ b/map/map_projection_loader/package.xml @@ -0,0 +1,31 @@ + + + + map_projection_loader + 0.1.0 + map_projection_loader package as a ROS 2 node + Koji Minoda + Yamato Ando + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + component_interface_specs + component_interface_utils + lanelet2_extension + rclcpp + rclcpp_components + tier4_map_msgs + + ament_cmake_gmock + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp new file mode 100644 index 0000000000000..77958c20a9e75 --- /dev/null +++ b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -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. + +#include "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include +#include +#include +#include + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); + if (!errors.empty()) { + throw std::runtime_error("Error occurred while loading lanelet2 map"); + } + + // If the lat & lon values in all the points of lanelet2 map are all zeros, + // it will be interpreted as a local map. + // If any single point exists with non-zero lat or lon values, it will be interpreted as MGRS. + bool is_local = true; + for (const auto & point : map->pointLayer) { + const auto gps_point = projector.reverse(point); + if (gps_point.lat != 0.0 || gps_point.lon != 0.0) { + is_local = false; + break; + } + } + + tier4_map_msgs::msg::MapProjectorInfo msg; + if (is_local) { + msg.type = "local"; + } else { + msg.type = "MGRS"; + msg.mgrs_grid = projector.getProjectedMGRSGrid(); + } + return msg; +} diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp new file mode 100644 index 0000000000000..2bd8a9cfa243a --- /dev/null +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -0,0 +1,72 @@ +// 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 "map_projection_loader/map_projection_loader.hpp" + +#include "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include + +#include + +#include + +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) +{ + YAML::Node data = YAML::LoadFile(filename); + + tier4_map_msgs::msg::MapProjectorInfo msg; + msg.type = data["type"].as(); + if (msg.type == "MGRS") { + msg.mgrs_grid = data["mgrs_grid"].as(); + } else if (msg.type == "LocalCartesianUTM") { + msg.map_origin.latitude = data["map_origin"]["latitude"].as(); + msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + } else if (msg.type == "local") { + ; // do nothing + } else { + throw std::runtime_error( + "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, and local"); + } + return msg; +} + +MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +{ + 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()); + 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"); + msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } + + // Publish the message + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(publisher_); + publisher_->publish(msg); +} diff --git a/map/map_projection_loader/src/map_projection_loader_node.cpp b/map/map_projection_loader/src/map_projection_loader_node.cpp new file mode 100644 index 0000000000000..1d9336be0d9dd --- /dev/null +++ b/map/map_projection_loader/src/map_projection_loader_node.cpp @@ -0,0 +1,23 @@ +// 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 "map_projection_loader/map_projection_loader.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/map/map_projection_loader/test/data/projection_info_local.yaml b/map/map_projection_loader/test/data/projection_info_local.yaml new file mode 100644 index 0000000000000..cea4aaf31d439 --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_local.yaml @@ -0,0 +1 @@ +type: local diff --git a/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml b/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml new file mode 100644 index 0000000000000..c7500d37bb35a --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_local_cartesian_utm.yaml @@ -0,0 +1,4 @@ +type: LocalCartesianUTM +map_origin: + latitude: 35.6762 + longitude: 139.6503 diff --git a/map/map_projection_loader/test/data/projection_info_mgrs.yaml b/map/map_projection_loader/test/data/projection_info_mgrs.yaml new file mode 100644 index 0000000000000..d7687521be5fa --- /dev/null +++ b/map/map_projection_loader/test/data/projection_info_mgrs.yaml @@ -0,0 +1,2 @@ +type: MGRS +mgrs_grid: 54SUE diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp new file mode 100644 index 0000000000000..206053fd2e8d2 --- /dev/null +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -0,0 +1,132 @@ +// 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 "map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include +#include + +#include + +#include + +void save_dummy_mgrs_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) +{ + int zone = std::stoi(mgrs_coord.substr(0, 2)); + bool is_north = false; + int precision = 0; + double utm_x{}; + double utm_y{}; + double lat{}; + double lon{}; + GeographicLib::MGRS::Reverse(mgrs_coord, zone, is_north, utm_x, utm_y, precision, false); + GeographicLib::UTMUPS::Reverse(zone, is_north, utm_x, utm_y, lat, lon); + + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << ""; + + file.close(); +} + +void save_dummy_local_lanelet2_map(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + +void save_dummy_mgrs_lanelet2_map_with_one_zero_point(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + +TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) +{ + // Save dummy lanelet2 map + const std::string mgrs_grid = "54SUE"; + const std::string mgrs_coord = mgrs_grid + "1000010000"; + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_mgrs_lanelet2_map(mgrs_coord, output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "MGRS"); + EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid); +} + +TEST(TestLoadFromLanelet2Map, LoadLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_local_lanelet2_map(output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "local"); +} + +TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_mgrs_lanelet2_map_with_one_zero_point(output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "MGRS"); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py new file mode 100644 index 0000000000000..920f5e3be11a1 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/projection_info_local_cartesian_utm.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadUTMFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py new file mode 100644 index 0000000000000..8ab0db43e32a7 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/projection_info_local.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadLocalFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py new file mode 100644 index 0000000000000..f18f9b7c17e75 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projection_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadMGRSFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projection_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projection_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.type, yaml_data["type"]) + self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index 2d65f75a098a3..4655cf4c2f73a 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -46,8 +46,7 @@ ament_target_dependencies(compare_map_segmentation sensor_msgs tier4_autoware_utils autoware_map_msgs - component_interface_specs - component_interface_utils + nav_msgs ) if(OPENMP_FOUND) diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md index ad891b9ad6afa..0566b5af806db 100644 --- a/perception/compare_map_segmentation/README.md +++ b/perception/compare_map_segmentation/README.md @@ -61,12 +61,11 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_ #### Input -| Name | Type | Description | -| ------------------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/map` | `sensor_msgs::msg::PointCloud2` | map (in case static map loading) | -| `~/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading) | -| `/localization/initialization_state` | `localization_interface::InitializationState` | Ego-vehicle pose initialization state (in dynamic map loading) | +| Name | Type | Description | +| ------------------------------- | ------------------------------- | ------------------------------------------------------ | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/map` | `sensor_msgs::msg::PointCloud2` | map (in case static map loading) | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | current ego-vehicle pose (in case dynamic map loading) | #### Output diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index 9a9c0d1534a5e..21cb19edcd0ec 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -15,12 +15,11 @@ #ifndef COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ #define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ -#include -#include #include #include #include +#include #include #include @@ -154,16 +153,11 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader }; typedef typename std::map VoxelGridDict; - using InitializationState = localization_interface::InitializationState; /** \brief Map to hold loaded map grid id and it's voxel filter */ VoxelGridDict current_voxel_grid_dict_; + rclcpp::Subscription::SharedPtr sub_kinematic_state_; - rclcpp::Subscription::SharedPtr - sub_estimated_pose_; - component_interface_utils::Subscription::SharedPtr - sub_pose_initializer_state_; - InitializationState::Message initialization_state_; std::optional current_position_ = std::nullopt; std::optional last_updated_position_ = std::nullopt; rclcpp::TimerBase::SharedPtr map_update_timer_; @@ -200,8 +194,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group); - void onEstimatedPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose); - void onPoseInitializerCallback(const InitializationState::Message::ConstSharedPtr msg); + void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose); void timer_callback(); bool should_update_map() const; @@ -267,7 +260,8 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader int index = static_cast( std::floor((kv.second.min_b_x - origin_x_) / map_grid_size_x_) + map_grids_x_ * std::floor((kv.second.min_b_y - origin_y_) / map_grid_size_y_)); - if (index >= map_grids_x_ * map_grids_y_) { + // TODO(1222-takeshi): check if index is valid + if (index >= map_grids_x_ * map_grids_y_ || index < 0) { continue; } current_voxel_grid_array_.at(index) = std::make_shared(kv.second); diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index fc6806ddaa843..42fad723b5d57 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -18,10 +18,9 @@ autoware_cmake autoware_map_msgs - component_interface_specs - component_interface_utils grid_map_pcl grid_map_ros + nav_msgs pcl_conversions pointcloud_preprocessor rclcpp diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index 426dc8e706dc9..06bd89af38259 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -301,13 +301,8 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( map_loader_radius_ = node->declare_parameter("map_loader_radius"); auto main_sub_opt = rclcpp::SubscriptionOptions(); main_sub_opt.callback_group = main_callback_group; - - const auto localization_node = component_interface_utils::NodeAdaptor(node); - localization_node.init_sub( - sub_pose_initializer_state_, this, &VoxelGridDynamicMapLoader::onPoseInitializerCallback); - - sub_estimated_pose_ = node->create_subscription( - "pose_with_covariance", rclcpp::QoS{1}, + sub_kinematic_state_ = node->create_subscription( + "kinematic_state", rclcpp::QoS{1}, std::bind(&VoxelGridDynamicMapLoader::onEstimatedPoseCallback, this, std::placeholders::_1), main_sub_opt); RCLCPP_INFO(logger_, "VoxelGridDynamicMapLoader initialized.\n"); @@ -327,18 +322,7 @@ VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( node, node->get_clock(), period_ns, std::bind(&VoxelGridDynamicMapLoader::timer_callback, this), timer_callback_group_); } -void VoxelGridDynamicMapLoader::onPoseInitializerCallback( - const InitializationState::Message::ConstSharedPtr msg) -{ - initialization_state_.state = msg->state; - if (msg->state != InitializationState::Message::INITIALIZED) { - current_position_ = std::nullopt; - last_updated_position_ = std::nullopt; - RCLCPP_INFO(logger_, "Initializing pose... Reset the position of Vehicle"); - } -} -void VoxelGridDynamicMapLoader::onEstimatedPoseCallback( - geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +void VoxelGridDynamicMapLoader::onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr msg) { current_position_ = msg->pose.pose.position; } @@ -417,9 +401,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map( } void VoxelGridDynamicMapLoader::timer_callback() { - if ( - current_position_ == std::nullopt || - initialization_state_.state != InitializationState::Message::INITIALIZED) { + if (current_position_ == std::nullopt) { return; } if (last_updated_position_ == std::nullopt) { diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index d4fca5c61295e..f882c6dd165fe 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -80,6 +80,11 @@ rclcpp_components_register_node(object_position_filter EXECUTABLE object_position_filter_node ) +rclcpp_components_register_node(occupancy_grid_based_validator + PLUGIN "occupancy_grid_based_validator::OccupancyGridBasedValidator" + EXECUTABLE occupancy_grid_based_validator_node +) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 8c9a28465038b..04bcbd87172b3 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -5,24 +5,23 @@ + + - - - - - + + + - diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index f4256422e6c8b..db86bbf80d250 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -57,7 +57,7 @@ def load_composable_node_param(param_path): ("map", LaunchConfiguration("input_map")), ("output", "compare_map_filtered/pointcloud"), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index a2c7c5966da62..82ce5605b67cd 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -47,7 +47,7 @@ def load_composable_node_param(param_path): ("map", LaunchConfiguration("input_map")), ("output", "map_filter/pointcloud"), ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), + ("kinematic_state", "/localization/kinematic_state"), ], parameters=[load_composable_node_param("compare_map_param_path")], ) diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index dcff2b33dcdb5..bdd793896062a 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -75,7 +75,7 @@ find_library(YAML_CPP_LIBRARY PATHS ${YAML_CPP_LIBRARY_DIRS}) link_directories(${YAML_CPP_LIBRARY_DIRS}) - +# cspell:ignore DHAVE, YAMLCPP if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") add_definitions(-DHAVE_NEW_YAMLCPP) endif() diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 8c0ba744491e7..906ad71d21732 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -124,7 +124,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) else() message(STATUS "diff ${FILE_NAME}") message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -132,7 +132,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) else() message(STATUS "not found ${FILE_NAME}") message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -145,8 +145,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) endfunction() # default model - download(pts_voxel_encoder_pointpainting.onnx e674271096f6fbbe0f808eef87f5a5ab) - download(pts_backbone_neck_head_pointpainting.onnx d4527a4da08959c2bf9c997112a1de35) + download(pts_voxel_encoder_pointpainting.onnx 25c70f76a45a64944ccd19f604c99410) + download(pts_backbone_neck_head_pointpainting.onnx 2c7108245240fbd7bf0104dd68225868) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 56346b5a3eb41..e1be5426cba4b 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,15 +1,16 @@ /**: ros__parameters: class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] - point_feature_size: 9 # x, y, z, time-lag and car, pedestrian, bicycle + 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: [-102.4, -102.4, -4.0, 102.4, 102.4, 6.0] - voxel_size: [0.32, 0.32, 10.0] + 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: 14 + 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.5 + circle_nms_dist_threshold: 0.3 iou_nms_target_class_names: ["CAR"] iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 diff --git a/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml new file mode 100755 index 0000000000000..bd49dc65749a9 --- /dev/null +++ b/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + # UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + passthrough_lower_bound_probability_thresholds: [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.50] + trust_distances: [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0] + min_iou_threshold: 0.5 + use_roi_probability: false + roi_probability_threshold: 0.5 + + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN <-roi_msg + [1, 0, 0, 0, 0, 0, 0, 0, # UNKNOWN <-detected_objects + 0, 1, 1, 1, 1, 0, 0, 0, # CAR + 0, 1, 1, 1, 1, 0, 0, 0, # TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, # BUS + 0, 1, 1, 1, 1, 0, 0, 0, # TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, # MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, # BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] # PEDESTRIAN diff --git a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md index accdc350bf256..4e054fc911dc6 100644 --- a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md @@ -41,14 +41,16 @@ The DetectedObject has three possible shape choices/implementations, where the p ### Core Parameters -| Name | Type | Description | -| ----------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings. | -| `min_iou_threshold` | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused. | -| `passthrough_lower_bound_probability_threshold` | double | If the `existence_probability` of a detected object is greater than the threshold, it is published in output. | -| `use_roi_probability` | float | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects. | -| `roi_probability_threshold` | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. | +| Name | Type | Description | +| ------------------------------------------------ | -------------- | -------------------------------------------------------------------------------------------------------------------------- | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings. | +| `passthrough_lower_bound_probability_thresholds` | vector[double] | If the `existence_probability` of a detected object is greater than the threshold, it is published in output. | +| `trust_distances` | vector[double] | If the distance of a detected object from the origin of frame_id is greater than the threshold, it is published in output. | +| `min_iou_threshold` | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused. | +| `use_roi_probability` | float | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects. | +| `roi_probability_threshold` | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. | +| `can_assign_matrix` | vector[int] | association matrix between rois and detected_objects to check that two rois on images can be match | ## Assumptions / Known limits 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 8168ad2c9f265..e11a62c060480 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 @@ -18,6 +18,8 @@ #include "image_projection_based_fusion/fusion_node.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" +#include "autoware_auto_perception_msgs/msg/object_classification.hpp" + #include #include #include @@ -40,15 +42,14 @@ class RoiDetectedObjectFusionNode : public FusionNode generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection); + std::map generateDetectedObjectRoIs( + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection); void fuseObjectsOnImage( - const std::vector & objects, + const DetectedObjects & input_object_msg, const std::vector & image_rois, - const std::map & object_roi_map); + const std::map & object_roi_map); void publish(const DetectedObjects & output_msg) override; @@ -57,13 +58,16 @@ class RoiDetectedObjectFusionNode : public FusionNode passthrough_lower_bound_probability_thresholds{}; + std::vector trust_distances{}; double min_iou_threshold{}; bool use_roi_probability{}; double roi_probability_threshold{}; + Eigen::MatrixXi can_assign_matrix; } fusion_params_; - std::vector passthrough_object_flags_, fused_object_flags_, ignored_object_flags_; + std::map> passthrough_object_flags_map_, fused_object_flags_map_, + ignored_object_flags_map_; }; } // namespace image_projection_based_fusion 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 0e18e9ee42a47..e6275a2ee83c7 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -41,7 +41,7 @@ - + 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 dd71ca462f666..b6165fc7c87d2 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 @@ -18,6 +18,7 @@ + @@ -39,13 +40,9 @@ + - - - - - diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 083a40044489b..32d7a5633b811 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -17,6 +17,7 @@ image_transport lidar_centerpoint message_filters + object_recognition_utils pcl_conversions pcl_ros rclcpp diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index f76af5060316c..1bc351b08c01b 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -166,8 +166,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ if (sub_std_pair_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); timer_->cancel(); - publish(*(sub_std_pair_.second)); postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); sub_std_pair_.second = nullptr; std::fill(is_fused_.begin(), is_fused_.end(), false); @@ -205,7 +205,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ // if matching rois exist, fuseOnSingle for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { - RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } @@ -260,8 +261,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ // Msg if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - publish(*output_msg); postprocess(*output_msg); + publish(*output_msg); std::fill(is_fused_.begin(), is_fused_.end(), false); sub_std_pair_.second = nullptr; @@ -298,7 +299,8 @@ void FusionNode::roiCallback( if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { - RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg; return; } @@ -322,8 +324,8 @@ void FusionNode::roiCallback( if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - publish(*(sub_std_pair_.second)); postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); std::fill(is_fused_.begin(), is_fused_.end(), false); sub_std_pair_.second = nullptr; @@ -362,8 +364,8 @@ void FusionNode::timer_callback() if (sub_std_pair_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); - publish(*(sub_std_pair_.second)); postprocess(*(sub_std_pair_.second)); + publish(*(sub_std_pair_.second)); // add processing time for debug if (debug_publisher_) { 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 3641b42db461a..5253ac192c786 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -29,6 +30,18 @@ #include +namespace +{ + +Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + namespace image_projection_based_fusion { @@ -102,8 +115,12 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); class_names_ = this->declare_parameter>("class_names"); + const auto paint_class_names = + this->declare_parameter>("paint_class_names"); std::vector classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"}; - if (std::find(class_names_.begin(), class_names_.end(), "TRUCK") != class_names_.end()) { + if ( + std::find(paint_class_names.begin(), paint_class_names.end(), "TRUCK") != + paint_class_names.end()) { isClassTable_["CAR"] = std::bind(&isCar, std::placeholders::_1); } else { isClassTable_["CAR"] = std::bind(&isVehicle, std::placeholders::_1); @@ -113,9 +130,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt isClassTable_["BICYCLE"] = std::bind(&isBicycle, std::placeholders::_1); isClassTable_["PEDESTRIAN"] = std::bind(&isPedestrian, std::placeholders::_1); for (const auto & cls : classes_) { - auto it = find(class_names_.begin(), class_names_.end(), cls); - if (it != class_names_.end()) { - int index = it - class_names_.begin(); + auto it = find(paint_class_names.begin(), paint_class_names.end(), cls); + if (it != paint_class_names.end()) { + int index = it - paint_class_names.begin(); class_index_[cls] = index + 1; } else { isClassTable_.erase(cls); @@ -180,6 +197,12 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + sensor_msgs::msg::PointCloud2 tmp; tmp = painted_pointcloud_msg; @@ -232,6 +255,12 @@ void PointPaintingFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + auto num_bbox = (input_roi_msg.feature_objects).size(); if (num_bbox == 0) { return; @@ -241,39 +270,38 @@ void PointPaintingFusionNode::fuseOnSingleImage( std::vector debug_image_points; // get transform from cluster frame id to camera optical frame id - geometry_msgs::msg::TransformStamped transform_stamped; + // geometry_msgs::msg::TransformStamped transform_stamped; + Eigen::Affine3f lidar2cam_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ painted_pointcloud_msg.header.frame_id, camera_info.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ painted_pointcloud_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } - transform_stamped = transform_stamped_optional.value(); + lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } // transform - sensor_msgs::msg::PointCloud2 transformed_pointcloud; - tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); - - std::chrono::system_clock::time_point start, end; - start = std::chrono::system_clock::now(); + // sensor_msgs::msg::PointCloud2 transformed_pointcloud; + // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); const auto x_offset = - transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::X)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::X)) .offset; const auto y_offset = - transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::Y)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Y)) .offset; const auto z_offset = - transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::Z)) + painted_pointcloud_msg.fields.at(static_cast(autoware_point_types::PointIndex::Z)) .offset; const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; - const auto p_step = transformed_pointcloud.point_step; + const auto p_step = painted_pointcloud_msg.point_step; // projection matrix Eigen::Matrix3f camera_projection; // use only x,y,z camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6); + Eigen::Vector3f point_lidar, point_camera; /** dc : don't care x | f x1 x2 dc ||xc| @@ -283,18 +311,24 @@ dc | dc dc dc dc ||zc| **/ auto objects = input_roi_msg.feature_objects; - int iterations = transformed_pointcloud.data.size() / transformed_pointcloud.point_step; + int iterations = painted_pointcloud_msg.data.size() / painted_pointcloud_msg.point_step; // iterate points // Requires 'OMP_NUM_THREADS=N' omp_set_num_threads(omp_num_threads_); #pragma omp parallel for for (int i = 0; i < iterations; i++) { int stride = p_step * i; - unsigned char * data = &transformed_pointcloud.data[0]; + unsigned char * data = &painted_pointcloud_msg.data[0]; unsigned char * output = &painted_pointcloud_msg.data[0]; float p_x = *reinterpret_cast(&data[stride + x_offset]); float p_y = *reinterpret_cast(&data[stride + y_offset]); float p_z = *reinterpret_cast(&data[stride + z_offset]); + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { continue; } @@ -342,6 +376,12 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte return; } + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + std::vector det_boxes3d; bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { 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 d9dce27fb0ecc..9789f52de5f74 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 @@ -37,8 +37,8 @@ namespace { const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32; // the same as max_point_in_voxel_size_ in config const std::size_t WARPS_PER_BLOCK = 4; -const std::size_t ENCODER_IN_FEATURE_SIZE = 14; // same as encoder_in_feature_size_ in config.hpp -const int POINT_FEATURE_SIZE = 9; +const std::size_t ENCODER_IN_FEATURE_SIZE = 12; // same as encoder_in_feature_size_ in config.hpp +const int POINT_FEATURE_SIZE = 7; // cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) 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 e0c346c3108a5..9180b18adeed8 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 @@ -191,38 +191,39 @@ void RoiClusterFusionNode::fuseOnSingleImage( max_iou = iou + iou_x + iou_y; } } - bool is_roi_label_known = feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - bool is_roi_existence_prob_higher = - output_cluster_msg.feature_objects.at(index).object.existence_probability <= - feature_obj.object.existence_probability; - if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) { - output_cluster_msg.feature_objects.at(index).object.classification = - feature_obj.object.classification; - - // Update existence_probability for fused objects - if ( - output_cluster_msg.feature_objects.at(index).object.existence_probability < - min_roi_existence_prob_) { - output_cluster_msg.feature_objects.at(index).object.existence_probability = - min_roi_existence_prob_; + if (!output_cluster_msg.feature_objects.empty()) { + bool is_roi_label_known = feature_obj.object.classification.front().label != + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + bool is_roi_existence_prob_higher = + output_cluster_msg.feature_objects.at(index).object.existence_probability <= + feature_obj.object.existence_probability; + if (iou_threshold_ < max_iou && is_roi_existence_prob_higher && is_roi_label_known) { + output_cluster_msg.feature_objects.at(index).object.classification = + feature_obj.object.classification; + + // Update existence_probability for fused objects + if ( + output_cluster_msg.feature_objects.at(index).object.existence_probability < + min_roi_existence_prob_) { + output_cluster_msg.feature_objects.at(index).object.existence_probability = + min_roi_existence_prob_; + } } - } - // fuse with unknown roi - - if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) { - output_cluster_msg.feature_objects.at(index).object.classification = - feature_obj.object.classification; - // Update existence_probability for fused objects - if ( - output_cluster_msg.feature_objects.at(index).object.existence_probability < - min_roi_existence_prob_) { - output_cluster_msg.feature_objects.at(index).object.existence_probability = - min_roi_existence_prob_; + // fuse with unknown roi + + if (unknown_iou_threshold_ < max_iou && is_roi_existence_prob_higher && !is_roi_label_known) { + output_cluster_msg.feature_objects.at(index).object.classification = + feature_obj.object.classification; + // Update existence_probability for fused objects + if ( + output_cluster_msg.feature_objects.at(index).object.existence_probability < + min_roi_existence_prob_) { + output_cluster_msg.feature_objects.at(index).object.existence_probability = + min_roi_existence_prob_; + } } } - debug_image_rois.push_back(feature_obj.feature.roi); } 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 37637f99f69c9..333ec7d7ed206 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 @@ -14,6 +14,8 @@ #include "image_projection_based_fusion/roi_detected_object_fusion/node.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" + #include #include @@ -25,28 +27,47 @@ namespace image_projection_based_fusion RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_detected_object_fusion", options) { - fusion_params_.passthrough_lower_bound_probability_threshold = - declare_parameter("passthrough_lower_bound_probability_threshold"); + fusion_params_.passthrough_lower_bound_probability_thresholds = + declare_parameter>("passthrough_lower_bound_probability_thresholds"); + fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); + fusion_params_.trust_distances = declare_parameter>("trust_distances"); fusion_params_.use_roi_probability = declare_parameter("use_roi_probability"); fusion_params_.roi_probability_threshold = declare_parameter("roi_probability_threshold"); - fusion_params_.min_iou_threshold = declare_parameter("min_iou_threshold"); + { + const auto can_assign_vector_tmp = declare_parameter>("can_assign_matrix"); + std::vector can_assign_vector(can_assign_vector_tmp.begin(), can_assign_vector_tmp.end()); + const int label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), label_num, label_num); + fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); + } } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { - passthrough_object_flags_.clear(); - passthrough_object_flags_.resize(output_msg.objects.size()); - fused_object_flags_.clear(); - fused_object_flags_.resize(output_msg.objects.size()); - ignored_object_flags_.clear(); - ignored_object_flags_.resize(output_msg.objects.size()); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; + passthrough_object_flags.resize(output_msg.objects.size()); + fused_object_flags.resize(output_msg.objects.size()); + ignored_object_flags.resize(output_msg.objects.size()); for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - if ( - output_msg.objects.at(obj_i).existence_probability > - fusion_params_.passthrough_lower_bound_probability_threshold) { - passthrough_object_flags_.at(obj_i) = true; + const auto & object = output_msg.objects.at(obj_i); + const auto label = object_recognition_utils::getHighestProbLabel(object.classification); + const auto pos = object_recognition_utils::getPose(object).position; + const auto object_sqr_dist = pos.x * pos.x + pos.y * pos.y; + const auto prob_threshold = + fusion_params_.passthrough_lower_bound_probability_thresholds.at(label); + const auto trust_sqr_dist = + fusion_params_.trust_distances.at(label) * fusion_params_.trust_distances.at(label); + if (object.existence_probability > prob_threshold || object_sqr_dist > trust_sqr_dist) { + passthrough_object_flags.at(obj_i) = true; } } + + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + passthrough_object_flags_map_.insert(std::make_pair(timestamp_nsec, passthrough_object_flags)); + fused_object_flags_map_.insert(std::make_pair(timestamp_nsec, fused_object_flags)); + ignored_object_flags_map_.insert(std::make_pair(timestamp_nsec, ignored_object_flags)); } void RoiDetectedObjectFusionNode::fuseOnSingleImage( @@ -58,8 +79,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( Eigen::Affine3d object2camera_affine; { const auto transform_stamped_optional = getTransformStamped( - tf_buffer_, /*target*/ camera_info.header.frame_id, - /*source*/ input_object_msg.header.frame_id, input_object_msg.header.stamp); + tf_buffer_, /*target*/ input_roi_msg.header.frame_id, + /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp); if (!transform_stamped_optional) { return; } @@ -73,9 +94,9 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( camera_info.p.at(11); const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg.objects, static_cast(camera_info.width), + input_object_msg, static_cast(camera_info.width), static_cast(camera_info.height), object2camera_affine, camera_projection); - fuseObjectsOnImage(input_object_msg.objects, input_roi_msg.feature_objects, object_roi_map); + fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); @@ -86,26 +107,35 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } } -std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const std::vector & input_objects, const double image_width, - const double image_height, const Eigen::Affine3d & object2camera_affine, - const Eigen::Matrix4d & camera_projection) +std::map +RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( + const DetectedObjects & input_object_msg, const double image_width, const double image_height, + const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection) { - std::map object_roi_map; - for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) { + std::map object_roi_map; + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (passthrough_object_flags_map_.size() == 0) { + return object_roi_map; + } + if (passthrough_object_flags_map_.count(timestamp_nsec) == 0) { + return object_roi_map; + } + const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; - { - const auto & object = input_objects.at(obj_i); + const auto & object = input_object_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { - continue; - } + if (passthrough_object_flags.at(obj_i)) { + continue; + } - // filter point out of scope - if (debugger_ && out_of_scope(object)) { - continue; - } + // filter point out of scope + if (debugger_ && out_of_scope(object)) { + continue; + } + { std::vector vertices; objectToVertices(object.kinematics.pose_with_covariance.pose, object.shape, vertices); transformPoints(vertices, object2camera_affine, vertices_camera_coord); @@ -142,7 +172,7 @@ std::map RoiDetectedObjectFusionNode::generateDet } } } - if (point_on_image_cnt == 0) { + if (point_on_image_cnt < 3) { continue; } @@ -151,16 +181,18 @@ std::map RoiDetectedObjectFusionNode::generateDet max_x = std::min(max_x, image_width - 1); max_y = std::min(max_y, image_height - 1); - // build roi - RegionOfInterest roi; - roi.x_offset = static_cast(min_x); - roi.y_offset = static_cast(min_y); - roi.width = static_cast(max_x) - static_cast(min_x); - roi.height = static_cast(max_y) - static_cast(min_y); - object_roi_map.insert(std::make_pair(obj_i, roi)); + DetectedObjectWithFeature object_roi; + object_roi.feature.roi.x_offset = static_cast(min_x); + object_roi.feature.roi.y_offset = static_cast(min_y); + object_roi.feature.roi.width = + static_cast(max_x) - static_cast(min_x); + object_roi.feature.roi.height = + static_cast(max_y) - static_cast(min_y); + object_roi.object = object; + object_roi_map.insert(std::make_pair(obj_i, object_roi)); if (debugger_) { - debugger_->obstacle_rois_.push_back(roi); + debugger_->obstacle_rois_.push_back(object_roi.feature.roi); } } @@ -168,13 +200,26 @@ std::map RoiDetectedObjectFusionNode::generateDet } void RoiDetectedObjectFusionNode::fuseObjectsOnImage( - const std::vector & objects __attribute__((unused)), + const DetectedObjects & input_object_msg, const std::vector & image_rois, - const std::map & object_roi_map) + const std::map & object_roi_map) { + int64_t timestamp_nsec = + input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec; + if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { + return; + } + if ( + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + for (const auto & object_pair : object_roi_map) { const auto & obj_i = object_pair.first; - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { continue; } @@ -182,7 +227,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( float max_iou = 0.0f; for (const auto & image_roi : image_rois) { const auto & object_roi = object_pair.second; - const double iou = calcIoU(object_roi, image_roi.feature.roi); + const auto object_roi_label = + object_recognition_utils::getHighestProbLabel(object_roi.object.classification); + const auto image_roi_label = + object_recognition_utils::getHighestProbLabel(image_roi.object.classification); + if (!fusion_params_.can_assign_matrix(object_roi_label, image_roi_label)) { + continue; + } + + const double iou = calcIoU(object_roi.feature.roi, image_roi.feature.roi); if (iou > max_iou) { max_iou = iou; roi_prob = image_roi.object.existence_probability; @@ -192,15 +245,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( if (max_iou > fusion_params_.min_iou_threshold) { if (fusion_params_.use_roi_probability) { if (roi_prob > fusion_params_.roi_probability_threshold) { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } else { - fused_object_flags_.at(obj_i) = true; + fused_object_flags.at(obj_i) = true; } } else { - ignored_object_flags_.at(obj_i) = true; + ignored_object_flags.at(obj_i) = true; } } } @@ -234,19 +287,37 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) return; } + int64_t timestamp_nsec = + output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec; + if ( + passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || + ignored_object_flags_map_.size() == 0) { + return; + } + if ( + passthrough_object_flags_map_.count(timestamp_nsec) == 0 || + fused_object_flags_map_.count(timestamp_nsec) == 0 || + ignored_object_flags_map_.count(timestamp_nsec) == 0) { + return; + } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; output_objects_msg.header = output_msg.header; debug_fused_objects_msg.header = output_msg.header; debug_ignored_objects_msg.header = output_msg.header; for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags_.at(obj_i)) { + if (passthrough_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); } - if (fused_object_flags_.at(obj_i)) { + if (fused_object_flags.at(obj_i)) { output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); - } else if (ignored_object_flags_.at(obj_i)) { + } + if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } @@ -255,6 +326,10 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + + passthrough_object_flags_map_.erase(timestamp_nsec); + fused_object_flags_map_.erase(timestamp_nsec); + ignored_object_flags_map_.erase(timestamp_nsec); } } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 1eaf04562b2b8..670c5596001fb 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -24,7 +24,7 @@ std::optional getTransformStamped( try { geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tf_buffer.lookupTransform( - target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.01)); return transform_stamped; } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(rclcpp::get_logger("image_projection_based_fusion"), ex.what()); 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 6a0bb29e9fb2d..f265633bd55dc 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 @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -31,6 +32,7 @@ #include #include #include +#include #include #include @@ -106,6 +108,7 @@ using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; using tier4_autoware_utils::StopWatch; +using tier4_debug_msgs::msg::StringStamped; class MapBasedPredictionNode : public rclcpp::Node { @@ -116,6 +119,7 @@ class MapBasedPredictionNode : public rclcpp::Node // ROS Publisher and Subscriber rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_debug_markers_; + rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; @@ -180,7 +184,7 @@ class MapBasedPredictionNode : public rclcpp::Node LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object, - const lanelet::BasicPoint2d & search_point); + const bool check_distance = true); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; void updateObjectData(TrackedObject & object); diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 33abfe814c498..0765490502ec4 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_debug_msgs unique_identifier_msgs visualization_msgs 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 a7762815f9e69..be6327cf5c01a 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -40,6 +40,8 @@ namespace map_based_prediction { +namespace +{ /** * @brief First order Low pass filtering * @@ -227,7 +229,7 @@ lanelet::ConstLanelets getRightLineSharingLanelets( for (auto & candidate : right_lane_candidates) { // exclude self lanelet if (candidate == current_lanelet) continue; - // if candidate has linestring as leftbound, assign it to output + // if candidate has linestring as left bound, assign it to output if (candidate.leftBound() == current_lanelet.rightBound()) { output_lanelets.push_back(candidate); } @@ -254,7 +256,7 @@ lanelet::ConstLanelets getLeftLineSharingLanelets( for (auto & candidate : left_lane_candidates) { // exclude self lanelet if (candidate == current_lanelet) continue; - // if candidate has linestring as rightbound, assign it to output + // if candidate has linestring as right bound, assign it to output if (candidate.rightBound() == current_lanelet.leftBound()) { output_lanelets.push_back(candidate); } @@ -287,7 +289,7 @@ lanelet::routing::LaneletPaths getPossiblePathsForIsolatedLanelet( lanelet::ConstLanelets possible_lanelets; possible_lanelets.push_back(lanelet); lanelet::routing::LaneletPaths possible_paths; - // need to init path with constlanelets + // need to initialize path with constant lanelets lanelet::routing::LaneletPath possible_path(possible_lanelets); possible_paths.push_back(possible_path); return possible_paths; @@ -586,6 +588,54 @@ ObjectClassification::_label_type changeLabelForPrediction( } } +StringStamped createStringStamped(const rclcpp::Time & now, const double data) +{ + StringStamped msg; + msg.stamp = now; + msg.data = std::to_string(data); + return msg; +} + +// NOTE: These two functions are copied from the route_handler package. +lanelet::Lanelets getRightOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet) +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr->laneletLayer.findUsages(lanelet.rightBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} + +lanelet::Lanelets getLeftOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet) +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr->laneletLayer.findUsages(lanelet.leftBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} +} // namespace + MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) { @@ -649,6 +699,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); + pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); } PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics( @@ -691,6 +742,7 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg) void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { + stop_watch_.tic(); // Guard for map pointer and frame transformation if (!lanelet_map_ptr_) { return; @@ -869,6 +921,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); pub_debug_markers_->publish(debug_markers); + const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc()); + pub_calculation_time_->publish(calculation_time_msg); } PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( @@ -1071,44 +1125,93 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob std::vector> surrounding_lanelets = lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10); - // No Closest Lanelets - if (surrounding_lanelets.empty()) { - return {}; + { // Step 1. Search same directional lanelets + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return {}; + } + + LaneletsData object_lanelets; + std::optional> closest_lanelet{std::nullopt}; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets and + // Check if similar lanelet is inside the object lanelet + if (!checkCloseLaneletCondition(lanelet, object) || isDuplicated(lanelet, object_lanelets)) { + continue; + } + + // Memorize closest lanelet + // NOTE: The object may be outside the lanelet. + if (!closest_lanelet || lanelet.first < closest_lanelet->first) { + closest_lanelet = lanelet; + } + + // Check if the obstacle is inside of this lanelet + constexpr double epsilon = 1e-3; + if (lanelet.first < epsilon) { + const auto object_lanelet = + LaneletData{lanelet.second, calculateLocalLikelihood(lanelet.second, object)}; + object_lanelets.push_back(object_lanelet); + } + } + + if (!object_lanelets.empty()) { + return object_lanelets; + } + if (closest_lanelet) { + return LaneletsData{LaneletData{ + closest_lanelet->second, calculateLocalLikelihood(closest_lanelet->second, object)}}; + } } - LaneletsData closest_lanelets; - for (const auto & lanelet : surrounding_lanelets) { - // Check if the close lanelets meet the necessary condition for start lanelets and - // Check if similar lanelet is inside the closest lanelet - if ( - !checkCloseLaneletCondition(lanelet, object, search_point) || - isDuplicated(lanelet, closest_lanelets)) { - continue; + { // Step 2. Search opposite directional lanelets + // Get opposite lanelets and calculate distance to search point. + std::vector> surrounding_opposite_lanelets; + for (const auto & surrounding_lanelet : surrounding_lanelets) { + for (const auto & left_opposite_lanelet : + getLeftOppositeLanelets(lanelet_map_ptr_, surrounding_lanelet.second)) { + const double distance = lanelet::geometry::distance2d(left_opposite_lanelet, search_point); + surrounding_opposite_lanelets.push_back(std::make_pair(distance, left_opposite_lanelet)); + } + for (const auto & right_opposite_lanelet : + getRightOppositeLanelets(lanelet_map_ptr_, surrounding_lanelet.second)) { + const double distance = lanelet::geometry::distance2d(right_opposite_lanelet, search_point); + surrounding_opposite_lanelets.push_back(std::make_pair(distance, right_opposite_lanelet)); + } } - LaneletData closest_lanelet; - closest_lanelet.lanelet = lanelet.second; - closest_lanelet.probability = calculateLocalLikelihood(lanelet.second, object); - closest_lanelets.push_back(closest_lanelet); + std::optional> opposite_closest_lanelet{std::nullopt}; + for (const auto & lanelet : surrounding_opposite_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets + // except for distance checking + if (!checkCloseLaneletCondition(lanelet, object, false)) { + continue; + } + + // Memorize closest lanelet + if (!opposite_closest_lanelet || lanelet.first < opposite_closest_lanelet->first) { + opposite_closest_lanelet = lanelet; + } + } + if (opposite_closest_lanelet) { + return LaneletsData{LaneletData{ + opposite_closest_lanelet->second, + calculateLocalLikelihood(opposite_closest_lanelet->second, object)}}; + } } - return closest_lanelets; + return LaneletsData{}; } bool MapBasedPredictionNode::checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object, - const lanelet::BasicPoint2d & search_point) + const bool check_distance) { // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { return false; } - // Step2. Check if the obstacle is inside of this lanelet - if (!lanelet::geometry::inside(lanelet.second, search_point)) { - return false; - } - // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); @@ -1124,7 +1227,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( } } - // Step3. Calculate the angle difference between the lane angle and obstacle angle + // Step2. Calculate the angle difference between the lane angle and obstacle angle const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet.second, object.kinematics.pose_with_covariance.pose.position); @@ -1132,18 +1235,19 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); const double abs_norm_delta = std::fabs(normalized_delta_yaw); - // Step4. Check if the closest lanelet is valid, and add all + // Step3. Check if the closest lanelet is valid, and add all // of the lanelets that are below max_dist and max_delta_yaw const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; const bool is_yaw_reversed = M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if ( - lanelet.first < dist_threshold_for_searching_lanelet_ && - (is_yaw_reversed || abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_)) { - return true; + if (check_distance && dist_threshold_for_searching_lanelet_ < lanelet.first) { + return false; + } + if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) { + return false; } - return false; + return true; } float MapBasedPredictionNode::calculateLocalLikelihood( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index bc97c9ba92d7e..f115d8c07f72f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -301,7 +301,7 @@ inline void convertConvexHullToBoundingBox( input_object.kinematics.pose_with_covariance.pose.position.x, input_object.kinematics.pose_with_covariance.pose.position.y}; const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d Rinv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); double max_x = 0; double max_y = 0; @@ -314,7 +314,7 @@ inline void convertConvexHullToBoundingBox( Eigen::Vector2d vertex{ input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y}; - const Eigen::Vector2d local_vertex = Rinv * (vertex - center); + const Eigen::Vector2d local_vertex = R_inv * (vertex - center); max_x = std::max(max_x, local_vertex.x()); max_y = std::max(max_y, local_vertex.y()); min_x = std::min(min_x, local_vertex.x()); @@ -328,7 +328,7 @@ inline void convertConvexHullToBoundingBox( const double width = max_y - min_y; const double height = max_z; const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; - const Eigen::Vector2d new_center = center + Rinv.transpose() * new_local_center; + const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; // set output parameters output_object = input_object; diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index edf480990c1fd..0f7fc50ed535f 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -35,7 +35,7 @@ $$ Kinematic bicycle model uses slip angle $\beta$ and velocity $v$ to calculate yaw update. The merit of using this model is that it can prevent unintended yaw rotation when the vehicle is stopped. -![kbmodel](image/kinematic_bicycle_model.png) +![kinematic_bicycle_model](image/kinematic_bicycle_model.png) - **state variable** - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ ) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index edd5e6d7a835f..2b0fff35729d0 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -55,7 +55,7 @@ BicycleTracker::BicycleTracker( float q_stddev_y = 0.6; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] float r_stddev_x = 0.6; // [m] float r_stddev_y = 0.4; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 7cb2772f006c3..73916742f17bb 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -54,7 +54,7 @@ BigVehicleTracker::BigVehicleTracker( float q_stddev_y = 1.5; // [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -150,7 +150,7 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.1; // under steered if smaller than 0.5 + double point_ratio = 0.2; // under steered if smaller than 0.5 lf_ = bounding_box_.length * point_ratio; lr_ = bounding_box_.length * (1.0 - point_ratio); } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 12a84a7c7929c..27c2a7b821eb6 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -54,7 +54,7 @@ NormalVehicleTracker::NormalVehicleTracker( float q_stddev_y = 1.0; // object coordinate [m/s] float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(20); // object coordinate [rad/(s*s)] + float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] diff --git a/perception/object_merger/README.md b/perception/object_merger/README.md index 62a95ada62b09..18bf12fa8cdb8 100644 --- a/perception/object_merger/README.md +++ b/perception/object_merger/README.md @@ -34,7 +34,7 @@ The successive shortest path algorithm is used to solve the data association pro | `max_rad_matrix` | double | Maximum angle table for data association | | `base_link_frame_id` | double | association frame | | `distance_threshold_list` | `std::vector` | Distance threshold for each class used in judging overlap. The class order depends on [ObjectClassification](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/ObjectClassification.idl). | -| `generalized_iou_threshold` | double | Generalized IoU threshold | +| `generalized_iou_threshold` | `std::vector` | Generalized IoU threshold for each class | ## Tips diff --git a/perception/object_merger/config/overlapped_judge.param.yaml b/perception/object_merger/config/overlapped_judge.param.yaml index 94882fae4f282..0410b77390187 100644 --- a/perception/object_merger/config/overlapped_judge.param.yaml +++ b/perception/object_merger/config/overlapped_judge.param.yaml @@ -3,3 +3,6 @@ distance_threshold_list: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #UNKNOWN + + generalized_iou_threshold: + [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1] diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 704f7859f6672..1e5b9fad9c9ca 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -76,7 +76,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node { double precision_threshold; double recall_threshold; - double generalized_iou_threshold; + std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; }; diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 1b89693b0f9f7..3418f7d5a5e61 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 17397b782892b..4f600ce8a4948 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -35,8 +35,11 @@ bool isUnknownObjectOverlapped( const autoware_auto_perception_msgs::msg::DetectedObject & unknown_object, const autoware_auto_perception_msgs::msg::DetectedObject & known_object, const double precision_threshold, const double recall_threshold, - std::map distance_threshold_map, const double generalized_iou_threshold) + std::map distance_threshold_map, + const std::map generalized_iou_threshold_map) { + const double generalized_iou_threshold = generalized_iou_threshold_map.at( + object_recognition_utils::getHighestProbLabel(known_object.classification)); const double distance_threshold = distance_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); @@ -95,7 +98,7 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio overlapped_judge_param_.recall_threshold = declare_parameter("recall_threshold_to_judge_overlapped", 0.5); overlapped_judge_param_.generalized_iou_threshold = - declare_parameter("generalized_iou_threshold"); + convertListToClassMap(declare_parameter>("generalized_iou_threshold")); // get distance_threshold_map from distance_threshold_list /** TODO(Shin-kyoto): diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index facb61d82abb0..035845772ce53 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -60,3 +60,19 @@ ament_auto_package( launch config ) + +# test +if(BUILD_TESTING) + # launch_testing + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/test_pointcloud_based_method.py) + + # gtest + ament_add_gtest(test_utils + test/test_utils.cpp + ) + target_link_libraries(test_utils + ${PCL_LIBRARIES} + ${PROJECT_NAME}_common + ) +endif() 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 355cc4bd8b5bd..b99335b0e09ef 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 @@ -171,6 +171,8 @@ def add_launch_arg(name: str, default_value=None): get_package_share_directory("probabilistic_occupancy_grid_map") + "/config/updater.param.yaml", ), + 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"), set_container_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 5b6e0d1c3c242..49bf228905dcc 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 @@ -116,7 +116,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg( "updater_param_file", get_package_share_directory("probabilistic_occupancy_grid_map") - + "/config/updater.param.yaml", + + "/config/binary_bayes_filter_updater.param.yaml", ), set_container_executable, set_container_mt_executable, diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index a6f4d46242a3f..a90ae78b03597 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -36,9 +36,14 @@ pointcloud_to_laserscan + ament_cmake_gtest ament_lint_auto autoware_lint_common - + launch + launch_ros + launch_testing + launch_testing_ament_cmake + launch_testing_ros ament_cmake diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index 741d333142c9d..f2cd6203b3685 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -25,8 +25,16 @@ bool transformPointcloud( const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output) { geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2.lookupTransform( - target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + // lookup transform + try { + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + rclcpp::get_logger("probabilistic_occupancy_grid_map"), "Failed to lookup transform: %s", + ex.what()); + return false; + } // transform pointcloud Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); pcl_ros::transformPointCloud(tf_matrix, input, output); diff --git a/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py new file mode 100644 index 0000000000000..093811d4cc213 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_pointcloud_based_method.py @@ -0,0 +1,269 @@ +# 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 struct +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 sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +PACKAGE_NAME = "probabilistic_occupancy_grid_map" +INPUT_TOPIC_RAW = "/raw" +INPUT_TOPIC_OBSTACLE = "/obstacle" + + +# test launcher +@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/pointcloud_based_occupancy_grid_map.launch.py" + ) + launch_args = [ + ("input/obstacle_pointcloud", INPUT_TOPIC_OBSTACLE), + ("input/raw_pointcloud", INPUT_TOPIC_RAW), + ] + # action to include launch file + test_launch_file = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource(launch_file_path), + launch_arguments=launch_args, + ) + + return launch.LaunchDescription( + [ + test_launch_file, + launch_testing.actions.ReadyToTest(), + ] + ) + + +# util functions +def get_pointcloud_msg(pts: list): + """Create ros2 point cloud message from list of points. + + Args: + pts (list): list of points [[x, y, z], ...] + + Returns: + PointCloud2: ros2 point cloud message + """ + msg = PointCloud2() + np_pts = np.array(pts, dtype=np.float32).reshape(-1, 3) + binary_pts = np_pts.tobytes() + + # set current time + now = rclpy.clock.Clock().now() + msg.header.stamp = now.to_msg() + msg.header.frame_id = "base_link" + msg.height = 1 + msg.width = np_pts.shape[0] + msg.fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + ] + msg.is_bigendian = False + msg.point_step = 12 + msg.row_step = msg.point_step * msg.width + msg.is_dense = True + # msg.data = b"" + # msg_data_list = [] + # for pt in pts: + # msg_data_list.append(struct.pack('fff', pt[0], pt[1], pt[2])) + # msg.data = b"".join(msg_data_list) + msg.data = binary_pts + return msg + + +def parse_pointcloud_msg(msg: PointCloud2): + """Parse ros2 point cloud message to list of points. + + Args: + msg (PointCloud2): ros2 point cloud message + + Returns: + list: list of points [[x, y, z], ...] + """ + pts = [] + for i in range(msg.width): + offset = msg.point_step * i + x, y, z = struct.unpack("fff", msg.data[offset : offset + 12]) + pts.append([x, y, z]) + return pts + + +def generate_static_transform_msg(): + """Generate static transform message from base_link to map. + + Returns: + TransformStamped: static transform message + """ + msg = TransformStamped() + msg.header.stamp = rclpy.clock.Clock().now().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 + + +# Test Node IO +class TestNodeIO(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("test_node_io") + # 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)) + + # util functions + def create_pub_sub(self): + # create publisher + pub_raw = self.node.create_publisher(PointCloud2, INPUT_TOPIC_RAW, 10) + pub_obstacle = self.node.create_publisher(PointCloud2, INPUT_TOPIC_OBSTACLE, 10) + + sensor_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + ) + # create subscriber + self.msg_buffer = [] + # subscribe to occupancy grid with buffer + sub = self.node.create_subscription( + OccupancyGrid, "/occupancy_grid", self.callback, qos_profile=sensor_qos + ) + return pub_raw, pub_obstacle, sub + + # test functions + def test_normal_input(self): + """Test normal input. + + input: normal pointcloud + output: normal ogm + """ + # wait for the node to be ready + time.sleep(3) + # min_height, max_height = -1.0, 2.0 + input_points = [[1.0, 1.0, 1.0], [2.0, 2.0, 2.0]] + pub_raw, pub_obstacle, sub = self.create_pub_sub() + # publish input pointcloud + pt_msg = get_pointcloud_msg(input_points) + pub_raw.publish(pt_msg) + pub_obstacle.publish(pt_msg) + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=3.0) + self.assertEqual(len(self.msg_buffer), 1) + + def test_null_input(self, proc_info): + """Test null input. + + input: null pointcloud + output: null ogm + """ + # wait for the node to be ready + time.sleep(3) + input_points = [] + pub_raw, pub_obstacle, sub = self.create_pub_sub() + # publish input pointcloud + pt_msg = get_pointcloud_msg(input_points) + pub_raw.publish(pt_msg) + pub_obstacle.publish(pt_msg) + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=3.0) + + # check if process is successfully terminated + nodes = self.node.get_node_names() + self.assertIn("occupancy_grid_map_node", nodes) + self.assertEqual(len(self.msg_buffer), 1) + + def test_null_input2(self, proc_info): + """Test null input. + + input: null pointcloud without even frame_id + output: null ogm + """ + # wait for the node to be ready + time.sleep(3) + input_points = [] + pub_raw, pub_obstacle, sub = self.create_pub_sub() + # publish input pointcloud + pt_msg = get_pointcloud_msg(input_points) + pt_msg.header.frame_id = "" + pub_raw.publish(pt_msg) + pub_obstacle.publish(pt_msg) + # try to subscribe output pointcloud once + rclpy.spin_once(self.node, timeout_sec=3.0) + + # check if process is successfully terminated + nodes = self.node.get_node_names() + self.assertIn("occupancy_grid_map_node", nodes) + self.assertEqual(len(self.msg_buffer), 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 new file mode 100644 index 0000000000000..3b1edca14e146 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -0,0 +1,92 @@ +// 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 + +#include +// pcl +#include + +#include +#include + +// autoware +#include "utils/utils.hpp" + +#include + +// create pointcloud function +pcl::PointCloud createPCLPointCloudWithIteratedHeight(const size_t width) +{ + pcl::PointCloud pcl_cloud; + pcl_cloud.width = width; + pcl_cloud.height = 1; // assume height is 1 + pcl_cloud.points.resize(pcl_cloud.width * pcl_cloud.height); + for (size_t i = 0; i < pcl_cloud.points.size(); ++i) { + pcl_cloud.points[i].x = 1.0; + pcl_cloud.points[i].y = 1.0; + pcl_cloud.points[i].z = static_cast(i); + } + pcl_cloud.header.frame_id = "base_link"; + return pcl_cloud; +} + +// mock buffer to avoid tf2_ros::Buffer::lookupTransform() error +class MockBuffer : public tf2_ros::Buffer +{ +public: + MockBuffer() : Buffer(std::make_shared(RCL_ROS_TIME)) {} + + // override lookupTransform() to avoid error + geometry_msgs::msg::TransformStamped lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time) const override + { + (void)target_frame; + (void)source_frame; + (void)time; + geometry_msgs::msg::TransformStamped dummy_transform; + return dummy_transform; + } +}; + +// test pointcloud cropping function +TEST(TestUtils, TestCropPointcloudByHeight) +{ + // mock buffer + MockBuffer mock_buffer; + // create pointcloud with pcl + const auto pcl_cloud_10 = createPCLPointCloudWithIteratedHeight(10); + const auto pcl_cloud_0 = createPCLPointCloudWithIteratedHeight(0); + // convert to ros msg + sensor_msgs::msg::PointCloud2 ros_cloud_10; + sensor_msgs::msg::PointCloud2 ros_cloud_0; + pcl::toROSMsg(pcl_cloud_10, ros_cloud_10); + pcl::toROSMsg(pcl_cloud_0, ros_cloud_0); + + // test buffer + sensor_msgs::msg::PointCloud2 test1_output, test2_output, test3_output; + + // case1: normal input, normal output + EXPECT_NO_THROW( + utils::cropPointcloudByHeight(ros_cloud_10, mock_buffer, "base_link", 0.0, 10.0, test1_output)); + + // case2: normal input, empty output + EXPECT_NO_THROW(utils::cropPointcloudByHeight( + ros_cloud_10, mock_buffer, "base_link", -2.0, -0.1, test2_output)); + + // case3: empty input, normal output + EXPECT_NO_THROW( + utils::cropPointcloudByHeight(ros_cloud_0, mock_buffer, "base_link", 0.0, 10.0, test3_output)); +} 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 5d3dd3b997f24..ab5e4eb5abe90 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 @@ -220,7 +220,7 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() compensated_velocity.y += odometry_data_->twist.twist.linear.y; compensated_velocity.z += odometry_data_->twist.twist.linear.z; } else { - RCLCPP_INFO(get_logger(), "Odometry data is not coming"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "Odometry data is not coming"); } } kinematics.twist_with_covariance.twist.linear.x = std::sqrt( diff --git a/perception/traffic_light_arbiter/CMakeLists.txt b/perception/traffic_light_arbiter/CMakeLists.txt index ab7ba25fe3ad2..181c2bec867d1 100644 --- a/perception/traffic_light_arbiter/CMakeLists.txt +++ b/perception/traffic_light_arbiter/CMakeLists.txt @@ -12,4 +12,4 @@ rclcpp_components_register_nodes(${PROJECT_NAME} TrafficLightArbiter ) -ament_auto_package(INSTALL_TO_SHARE launch) +ament_auto_package(INSTALL_TO_SHARE launch config) diff --git a/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml new file mode 100644 index 0000000000000..5dc2b62eaa446 --- /dev/null +++ b/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + external_time_tolerance: 5.0 + perception_time_tolerance: 1.0 + external_priority: false diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp index afb5984c288fc..485ce84c5fea6 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -22,6 +22,7 @@ #include +#include #include class TrafficLightArbiter : public rclcpp::Node @@ -45,7 +46,7 @@ class TrafficLightArbiter : public rclcpp::Node void onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg); void arbitrateAndPublish(const builtin_interfaces::msg::Time & stamp); - std::unordered_set map_regulatory_elements_set_; + std::unique_ptr> map_regulatory_elements_set_; double external_time_tolerance_; double perception_time_tolerance_; diff --git a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml b/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml index 42944450378d6..eca4b8a61b5af 100644 --- a/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml +++ b/perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml @@ -1,11 +1,17 @@ + + + + + - - - + + + + diff --git a/perception/traffic_light_arbiter/package.xml b/perception/traffic_light_arbiter/package.xml index 7eb45f6c6b8cc..a95df132de3bd 100644 --- a/perception/traffic_light_arbiter/package.xml +++ b/perception/traffic_light_arbiter/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_arbiter package Kenzo Lobos-Tsunekawa + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index eb940fc18c32d..3baefaa43163a 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -72,9 +72,10 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg) lanelet::utils::conversion::fromBinMsg(*msg, map); const auto signals = lanelet::filter_traffic_signals(map); - map_regulatory_elements_set_.clear(); + map_regulatory_elements_set_ = std::make_unique>(); + for (const auto & signal : signals) { - map_regulatory_elements_set_.emplace(signal->id()); + map_regulatory_elements_set_->emplace(signal->id()); } } @@ -98,7 +99,7 @@ void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr if ( (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() > perception_time_tolerance_) { - latest_external_msg_.signals.clear(); + latest_perception_msg_.signals.clear(); } arbitrateAndPublish(msg->stamp); @@ -109,14 +110,22 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim using ElementAndPriority = std::pair; std::unordered_map> regulatory_element_signals_map; - if (map_regulatory_elements_set_.empty()) { + if (map_regulatory_elements_set_ == nullptr) { RCLCPP_WARN(get_logger(), "Received traffic signal messages before a map"); return; } + TrafficSignalArray output_signals_msg; + output_signals_msg.stamp = stamp; + + if (map_regulatory_elements_set_->empty()) { + pub_->publish(output_signals_msg); + return; + } + auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; - if (!map_regulatory_elements_set_.count(id)) { + if (!map_regulatory_elements_set_->count(id)) { RCLCPP_WARN( get_logger(), "Received a traffic signal not present in the current map (%lu)", id); return; @@ -138,14 +147,14 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim const auto get_highest_confidence_elements = [](const std::vector & elements_and_priority_vector) { - using Key = std::tuple; + using Key = Element::_shape_type; std::map highest_score_element_and_priority_map; std::vector highest_score_elements_vector; for (const auto & elements_and_priority : elements_and_priority_vector) { const auto & element = elements_and_priority.first; const auto & element_priority = elements_and_priority.second; - const auto key = std::make_tuple(element.color, element.shape); + const auto key = element.shape; auto [iter, success] = highest_score_element_and_priority_map.try_emplace(key, elements_and_priority); const auto & iter_element = iter->second.first; @@ -165,8 +174,6 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim return highest_score_elements_vector; }; - TrafficSignalArray output_signals_msg; - output_signals_msg.stamp = stamp; output_signals_msg.signals.reserve(regulatory_element_signals_map.size()); for (const auto & [regulatory_element_id, elements] : regulatory_element_signals_map) { 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 e502aac8e2d35..d4794443d95d9 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -2,6 +2,9 @@ + + + @@ -16,9 +19,9 @@ - - - + + + diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 442aa3f45ea88..5ce7840c28fd6 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -1,6 +1,6 @@ - - + + diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/traffic_light_fine_detector/package.xml index 960a90379279a..c54912e256c02 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/traffic_light_fine_detector/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_fine_detector package Mingyu Li + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_multi_camera_fusion/README.md b/perception/traffic_light_multi_camera_fusion/README.md index 2f225f1540f63..f7ee294cda147 100644 --- a/perception/traffic_light_multi_camera_fusion/README.md +++ b/perception/traffic_light_multi_camera_fusion/README.md @@ -9,12 +9,13 @@ ## Input topics -For every camera, the following three topics are subscribed: -| Name | Type | | -| ---------------------------------------| -------------------------------------------------------|----------------------------------------------------| -| `~//camera_info` | sensor_msgs::CameraInfo |camera info from traffic_light_map_based_detector | -| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray |detection roi from traffic_light_fine_detector | -| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray |classification result from traffic_light_classifier | +For every camera, the following three topics are subscribed: + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------- | --------------------------------------------------- | +| `~//camera_info` | sensor_msgs::CameraInfo | camera info from traffic_light_map_based_detector | +| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray | detection roi from traffic_light_fine_detector | +| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray | classification result from traffic_light_classifier | You don't need to configure these topics manually. Just provide the `camera_namespaces` parameter and the node will automatically extract the `` and create the subscribers. diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp index 4cd5569129a00..bdfd07ae54b28 100644 --- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp +++ b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp @@ -113,7 +113,7 @@ class MultiCameraFusion : public rclcpp::Node /* the mapping from traffic light id (instance id) to regulatory element id (group id) */ - std::map traffic_light_id_to_regulatory_ele_id_; + std::map> traffic_light_id_to_regulatory_ele_id_; /* save record arrays by increasing timestamp order. use multiset in case there are multiple cameras publishing images at exactly the same time diff --git a/perception/traffic_light_multi_camera_fusion/package.xml b/perception/traffic_light_multi_camera_fusion/package.xml index 6dd0bf89bd716..eb0858152e69e 100644 --- a/perception/traffic_light_multi_camera_fusion/package.xml +++ b/perception/traffic_light_multi_camera_fusion/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_multi_camera_fusion package Mingyu Li + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp index 387b3ef1f6758..6c29be9858a9a 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp @@ -216,7 +216,7 @@ void MultiCameraFusion::mapCallback( auto lights = tl->trafficLights(); for (const auto & light : lights) { - traffic_light_id_to_regulatory_ele_id_[light.id()] = tl->id(); + traffic_light_id_to_regulatory_ele_id_[light.id()].emplace_back(tl->id()); } } } @@ -302,11 +302,15 @@ void MultiCameraFusion::groupFusion( if (traffic_light_id_to_regulatory_ele_id_.count(roi_id) == 0) { RCLCPP_WARN_STREAM( get_logger(), "Found Traffic Light Id = " << roi_id << " which is not defined in Map"); - } else { - /* - keep the best record for every regulatory element id - */ - IdType reg_ele_id = traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id]; + continue; + } + + /* + keep the best record for every regulatory element id + */ + const auto reg_ele_id_vec = + traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id]; + for (const auto & reg_ele_id : reg_ele_id_vec) { if ( grouped_record_map.count(reg_ele_id) == 0 || ::compareRecord(p.second, grouped_record_map[reg_ele_id]) >= 0) { diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 154b650669c8d..0ae390c90b9a9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_occlusion_predictor package Mingyu Li + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 36f09be943144..366820a725018 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -117,7 +117,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( 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(in_roi_msg->rois.size(), 0); + occlusion_ratios.resize(out_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_, diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 8bb4947c56e3b..0352096d02b2b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -30,7 +30,8 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp - src/utils/safety_check.cpp + src/utils/path_safety_checker/safety_check.cpp + src/utils/path_safety_checker/objects_filtering.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp @@ -43,6 +44,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/start_planner/util.cpp src/utils/start_planner/shift_pull_out.cpp src/utils/start_planner/geometric_pull_out.cpp + src/utils/start_planner/freespace_pull_out.cpp src/utils/path_shifter/path_shifter.cpp src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 3a36fcea62e41..9ecc1a63b55b0 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -4,18 +4,15 @@ avoidance: resample_interval_for_planning: 0.3 # [m] resample_interval_for_output: 4.0 # [m] - detection_area_right_expand_dist: 0.0 # [m] - detection_area_left_expand_dist: 1.0 # [m] drivable_area_right_bound_offset: 0.0 # [m] drivable_area_left_bound_offset: 0.0 # [m] # avoidance module common setting enable_bound_clipping: false - enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false - enable_safety_check: true enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false + enable_cancel_maneuver: false disable_path_update: false # drivable area setting @@ -133,12 +130,27 @@ # For safety check safety_check: + # safety check configuration + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] + # collision check parameters + check_all_predicted_path: false # [-] + time_resolution: 0.5 # [s] + time_horizon: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] safety_check_hysteresis_factor: 2.0 # [-] - safety_check_ego_offset: 1.0 # [m] + # rss parameters + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.8 # [s] # For avoidance maneuver avoidance: @@ -147,12 +159,14 @@ lateral_execution_threshold: 0.499 # [m] lateral_small_shift_threshold: 0.101 # [m] lateral_avoid_check_threshold: 0.1 # [m] - road_shoulder_safety_margin: 0.3 # [m] + soft_road_shoulder_margin: 0.8 # [m] + hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] + remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] @@ -167,16 +181,29 @@ max_distance: 20.0 # [m] stop_buffer: 1.0 # [m] - constraints: - # vehicle slows down under longitudinal constraints - use_constraints_for_decel: false # [-] + policy: + # policy for vehicle slow down behavior. select "best_effort" or "reliable". + # "best_effort": slow down deceleration & jerk are limited by constraints. + # but there is a possibility that the vehicle can't stop in front of the vehicle. + # "reliable": insert stop or slow down point with ignoring decel/jerk constraints. + # make it possible to increase chance to avoid but uncomfortable deceleration maybe happen. + deceleration: "best_effort" # [-] + # policy for avoidance lateral margin. select "best_effort" or "reliable". + # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal + # margin to avoid. + # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid + # with expected lateral margin. + lateral_margin: "best_effort" # [-] + # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. + use_shorten_margin_immediately: true # [-] + 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] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] # longitudinal constraints longitudinal: @@ -186,12 +213,6 @@ max_jerk: 1.0 # [m/sss] max_acceleration: 1.0 # [m/ss] - target_velocity_matrix: - col_size: 2 - matrix: - [2.78, 13.9, # velocity [m/s] - 0.50, 1.00] # margin [m] - shift_line_pipeline: trim: quantize_filter_threshold: 0.2 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 02dc13ffd84da..95c60612bfdb7 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,19 +25,6 @@ visualize_maximum_drivable_area: true - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 # [s] - - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 diff --git a/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml new file mode 100644 index 0000000000000..896c44c9cec9a --- /dev/null +++ b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + dynamic_avoidance: + common: + enable_debug_info: true + + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: true + pedestrian: false + + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 + + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + + crossing_object: + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 + + front_object: + max_object_angle: 0.785 + + drivable_area_generation: + lat_offset_from_obstacle: 0.8 # [m] + max_lat_offset_to_avoid: 0.5 # [m] + + # for same directional object + overtaking_object: + max_time_to_collision: 40.0 # [s] + start_duration_to_avoid: 2.0 # [s] + end_duration_to_avoid: 4.0 # [s] + duration_to_hold_avoidance: 3.0 # [s] + + # for opposite directional object + oncoming_object: + max_time_to_collision: 15.0 # [s] + start_duration_to_avoid: 12.0 # [s] + end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 845f9c38e269b..701f05eb89ba1 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -1,6 +1,10 @@ /**: ros__parameters: dynamic_avoidance: + common: + enable_debug_info: true + use_hatched_road_markings: true + # avoidance is performed for the object type with true target_object: car: true @@ -15,30 +19,43 @@ min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 min_obj_lat_offset_to_ego_path: 0.0 # [m] max_obj_lat_offset_to_ego_path: 1.0 # [m] + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + crossing_object: - min_object_vel: 1.0 - max_object_angle: 1.05 + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 0.0 + max_oncoming_object_angle: 0.523 front_object: max_object_angle: 0.785 drivable_area_generation: - lat_offset_from_obstacle: 1.0 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] + max_time_for_object_lat_shift: 2.0 # [s] + lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] # for same directional object overtaking_object: - max_time_to_collision: 10.0 # [s] + max_time_to_collision: 40.0 # [s] start_duration_to_avoid: 2.0 # [s] end_duration_to_avoid: 4.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: - max_time_to_collision: 15.0 # [s] + max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 3c5846edf3ee9..39d05a7fb761b 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -31,7 +31,8 @@ # object recognition object_recognition: use_object_recognition: true - object_recognition_collision_check_margin: 1.0 + 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 # pull over pull_over: diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 8fabe8daa85b2..925e70d9084d6 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -8,7 +8,6 @@ lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] - lateral_acc_switching_velocity: 4.0 #[m/s] minimum_lane_changing_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] @@ -27,6 +26,18 @@ min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + # safety check + safety_check: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 5ae5c99ff8224..bea78664c65a3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -6,6 +6,7 @@ th_stopped_time: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 + th_moving_object_velocity: 1.0 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: false @@ -35,3 +36,39 @@ th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 + # freespace planner + freespace_planner: + enable_freespace_planner: true + end_pose_search_start_distance: 20.0 + end_pose_search_end_distance: 30.0 + end_pose_search_interval: 2.0 + freespace_planner_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 7436c26533000..693d610951bbb 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -485,15 +485,22 @@ The hatched road marking is defined on Lanelet map. See [here](https://github.co ### Safety check -The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable_safety_check` as `true`. +The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable` as `true`. ```yaml -enable_safety_check: false - -# For safety check +# safety check configuration +enable: true # [-] +check_current_lane: false # [-] +check_shift_side_lane: true # [-] +check_other_side_lane: false # [-] +check_unavoidable_object: false # [-] +check_other_object: true # [-] + +# collision check parameters +check_all_predicted_path: false # [-] +time_horizon: 10.0 # [s] +idling_time: 1.5 # [s] safety_check_backward_distance: 50.0 # [m] -safety_check_time_horizon: 10.0 # [s] -safety_check_idling_time: 1.5 # [s] safety_check_accel_for_rss: 2.5 # [m/ss] ``` @@ -594,13 +601,13 @@ $$ ### Avoidance cancelling maneuver -If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows: +If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows: - If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled. - If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled. - If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled. -If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. +If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. ## How to keep the consistency of the optimize-base path generation logic @@ -614,18 +621,15 @@ The avoidance specific parameter configuration file can be located at `src/autow namespace: `avoidance.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_safety_check | [-] | bool | Flag to enable safety check. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | - -**NOTE:** It has to set both `enable_safety_check` and `enable_yield_maneuver` to enable yield maneuver. +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | +| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | +| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | +| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | +| enable_cancel_maneuver | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | +| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | | Name | Unit | Type | Description | Default value | | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | @@ -695,14 +699,20 @@ namespace: `avoidance.target_filtering.` namespace: `avoidance.safety_check.` -| Name | Unit | Type | Description | Default value | -| :----------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | -| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | -| safety_check_time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | -| safety_check_idling_time | [t] | double | Time delay constant that be use for longitudinal margin calculation based on RSS. | 1.5 | -| safety_check_accel_for_rss | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS. | 2.5 | -| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | -| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | +| enable | [-] | bool | Enable to use safety check feature. | true | +| check_current_lane | [-] | bool | Check objects on current driving lane. | false | +| check_shift_side_lane | [-] | bool | Check objects on shift side lane. | true | +| check_other_side_lane | [-] | bool | Check objects on other side lane. | false | +| check_unavoidable_object | [-] | bool | Check collision between ego and unavoidable objects. | false | +| check_other_object | [-] | bool | Check collision between ego and non avoidance target objects. | false | +| check_all_predicted_path | [-] | bool | Check all prediction path of safety check target objects. | false | +| time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | +| time_resolution | [s] | double | Time resolution to check lateral/longitudinal margin is enough or not. | 0.5 | +| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | +| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | +| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | ### Avoidance maneuver parameters @@ -812,7 +822,7 @@ Developers can see what is going on in each process by visualizing all the avoid ![fig1](../image/avoidance/avoidance-debug-marker.png) -To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.output_debug_marker true` (no restart is needed) or simply set the `output_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`. +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`. ### Echoing debug message to find out why the objects were ignored diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 008087ee32b4b..99dadc959f5a5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -150,10 +150,11 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for object recognition based collision check -| Name | Unit | Type | Description | Default value | -| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | | 1.0 | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance | ## **Goal Search** diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index ae49542b3970b..fddd535931d94 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -157,6 +157,14 @@ stop See [safety check utils explanation](../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) + +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. + ##### 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. @@ -171,21 +179,13 @@ When driving on the public road with other vehicles, there exist scenarios where ```C++ lane_changing_time = f(shift_length, lat_acceleration, lat_jerk) -minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + backward_length_buffer_for_end_of_lane +minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + lane_change_finish_judge_buffer ``` 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) -#### Intersection - -Lane change in the intersection is prohibited following traffic regulation. Therefore, if the goal is place close passed the intersection, the lane change needs to be completed before ego vehicle enters the intersection region. Similar to the lane blocked case, in case of the path is unsafe, ego vehicle will stop and waits for the dynamic object to pass by. - -The following figure illustrate the intersection case. - -![intersection](../image/lane_change/lane_change-intersection_case.png) - ### Aborting lane change The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. @@ -209,11 +209,11 @@ while(Lane Following) if (Is Abort Condition Satisfied) then (**NO**) else (**YES**) if (Is Enough margin to retry lane change) then (**YES**) - if (Ego not depart from current lane yet) then (**YES**) + if (Ego is on lane change prepare phase) then (**YES**) :Cancel lane change; break else (**NO**) - if (Can perform abort maneuver) then (**YES**) + if (Will the overhang to target lane be less than threshold?) then (**YES**) :Perform abort maneuver; break else (NO) @@ -264,23 +264,40 @@ The last behavior will also occur if the ego vehicle has departed from the curre The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------- | ------------- | -| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `lane_changing_safety_check_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | -| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | -| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | -| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------------ | ------ | ------- | --------------------------------------------------------------------------------------------------------------- | ------------------ | +| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `longitudinal_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5 | +| `lateral_acceleration_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | +| `object_check_min_road_shoulder_width` | [m] | double | Vehicles around the center line within this distance will be excluded from parking objects | 0.5 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | +| `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | +| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | +| `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | +| `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | +| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.15, 0.15, 0.15] | +| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.5, 0.5, 0.5] | +| `target_object.car` | [-] | boolean | Include car objects for safety check | true | +| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | +| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | +| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | +| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | +| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | +| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | +| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | ### Collision checks during lane change -The following parameters are configurable in `behavior_path_planner.param.yaml`. +The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. | Name | Unit | Type | Description | Default value | | :----------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | @@ -292,7 +309,8 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`. | `rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 2.0 | | `enable_collision_check_at_prepare_phase` | [-] | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase. | true | | `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | -| `use_predicted_path_outside_lanelet` | [-] | boolean | If true, include collision check for predicted path that is out of lanelet (freespace). | false | +| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module include objects on current lanes. | true | +| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | | `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | (\*1) the value must be negative. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md index 2d8ec7a6b2de3..30e2093cb465e 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner/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/safety_check/safety_check_flow.drawio.svg) +![safety_check_flow](../image/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/safety_check/front_object.drawio.svg) +![front_object](../image/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/safety_check/extended_polygons.drawio.svg) +![extended_polygons](../image/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/image/lane_change/lane_objects.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg new file mode 100644 index 0000000000000..768c50d034394 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_objects.drawio.svg @@ -0,0 +1,150 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + Current Lane + +
+
+
+ + Current Lane + + + + + + +
+
+
+ + Target Lane + +
+
+
+ + Target Lane + + + + + + +
+
+
+ + Other Lane + +
+
+
+ + Other Lane + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/safety_check/extended_polygons.drawio.svg b/planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/safety_check/extended_polygons.drawio.svg rename to planning/behavior_path_planner/image/path_safety_checker/extended_polygons.drawio.svg diff --git a/planning/behavior_path_planner/image/safety_check/front_object.drawio.svg b/planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/safety_check/front_object.drawio.svg rename to planning/behavior_path_planner/image/path_safety_checker/front_object.drawio.svg diff --git a/planning/behavior_path_planner/image/safety_check/safety_check_flow.drawio.svg b/planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg similarity index 100% rename from planning/behavior_path_planner/image/safety_check/safety_check_flow.drawio.svg rename to planning/behavior_path_planner/image/path_safety_checker/safety_check_flow.drawio.svg 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 e6ed8b7063d2c..54cfccc494e8a 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 @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::StopReasonArray; using visualization_msgs::msg::Marker; @@ -102,6 +104,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr stop_reason_publisher_; + rclcpp::Publisher::SharedPtr reroute_availability_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -171,6 +174,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; + /** + * @brief publish reroute availability + */ + void publish_reroute_availability(); + /** * @brief publish steering factor from intersection */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index 9e9ad5361fed8..ac24591a0a283 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -49,9 +49,6 @@ using visualization_msgs::msg::MarkerArray; MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); -MarkerArray createSafetyCheckMarkerArray( - const AvoidanceState & state, const Pose & pose, const DebugData & data); - MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, const float & b, const double & w); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 6894edaece828..b3cb53ec18349 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -122,7 +122,7 @@ MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3 MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, - const float & g, const float & b); + const float & g, const float & b, const float & w = 0.3); MarkerArray createObjectsMarkerArray( const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 1fdf4e020d71f..d6b99dca70618 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -100,7 +100,6 @@ struct BehaviorPathPlannerParameters // lane change parameters double lane_changing_lateral_jerk{0.5}; - double lateral_acc_switching_velocity{0.4}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; double lane_change_finish_judge_buffer{3.0}; @@ -142,20 +141,6 @@ struct BehaviorPathPlannerParameters // maximum drivable area visualization bool visualize_maximum_drivable_area; - - // collision check - double lateral_distance_max_threshold; - double longitudinal_distance_min_threshold; - double longitudinal_velocity_delta_time; - - double expected_front_deceleration; // brake parameter under normal lane change - double expected_rear_deceleration; // brake parameter under normal lane change - - double expected_front_deceleration_for_abort; // hard brake parameter for abort - double expected_rear_deceleration_for_abort; // hard brake parameter for abort - - double rear_vehicle_reaction_time; - double rear_vehicle_safety_time_margin; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ 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 8820ef70c37bf..29857590d4eda 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 @@ -105,9 +105,9 @@ class PlannerManager */ void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr) { - RCLCPP_INFO(logger_, "register %s module", manager_ptr->getModuleName().c_str()); + RCLCPP_INFO(logger_, "register %s module", manager_ptr->name().c_str()); manager_ptrs_.push_back(manager_ptr); - processing_time_.emplace(manager_ptr->getModuleName(), 0.0); + processing_time_.emplace(manager_ptr->name(), 0.0); } /** @@ -126,10 +126,10 @@ class PlannerManager */ void reset() { + std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); approved_module_ptrs_.clear(); candidate_module_ptrs_.clear(); root_lanelet_ = boost::none; - std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); resetProcessingTime(); } @@ -214,6 +214,16 @@ class PlannerManager return stop_reason_array; } + /** + * @brief check if there are approved modules. + */ + bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); } + + /** + * @brief check if there are candidate modules. + */ + bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); } + /** * @brief reset root lanelet. if there are approved modules, don't reset root lanelet. * @param planner data. @@ -301,8 +311,9 @@ class PlannerManager */ void deleteExpiredModules(SceneModulePtr & module_ptr) const { - const auto manager = getManager(module_ptr); - manager->deleteModules(module_ptr); + module_ptr->onExit(); + module_ptr->publishRTCStatus(); + module_ptr.reset(); } /** @@ -348,41 +359,21 @@ class PlannerManager candidate_module_ptrs_.clear(); } - /** - * @brief stop and remove not RUNNING modules in candidate_module_ptrs_. - */ - void clearNotRunningCandidateModules() - { - const auto it = std::remove_if( - candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) { - if (m->getCurrentStatus() != ModuleStatus::RUNNING) { - deleteExpiredModules(m); - return true; - } - return false; - }); - candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end()); - } - - /** - * @brief check if there is any RUNNING module in candidate_module_ptrs_. - */ - bool hasAnyRunningCandidateModule() - { - return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) { - return m->getCurrentStatus() == ModuleStatus::RUNNING; - }); - } - /** * @brief get current root lanelet. the lanelet is used for reference path generation. * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const + lanelet::ConstLanelet updateRootLanelet( + const std::shared_ptr & data, bool success_lane_change = false) const { lanelet::ConstLanelet ret{}; - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + if (success_lane_change) { + data->route_handler->getClosestPreferredLaneletWithinRoute( + data->self_odometry->pose.pose, &ret); + } else { + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + } RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } @@ -396,7 +387,7 @@ class PlannerManager { const auto itr = std::find_if( manager_ptrs_.begin(), manager_ptrs_.end(), - [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); }); + [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); }); if (itr == manager_ptrs_.end()) { throw std::domain_error("unknown manager name."); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index ee81915ea7944..7889e91767155 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include @@ -48,10 +49,8 @@ class AvoidanceModule : public SceneModuleInterface public: AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map); - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -62,13 +61,19 @@ class AvoidanceModule : public SceneModuleInterface void updateData() override; void acceptVisitor(const std::shared_ptr & visitor) const override; - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } std::shared_ptr get_debug_msg_array() const; private: + bool canTransitSuccessState() override; + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return true; } + /** * @brief update RTC status for candidate shift line. * @param candidate path. @@ -177,6 +182,11 @@ class AvoidanceModule : public SceneModuleInterface */ void initRTCStatus(); + /** + * @brief update RTC status. + */ + void updateRTCData(); + // ego state check /** @@ -185,19 +195,6 @@ class AvoidanceModule : public SceneModuleInterface */ AvoidanceState updateEgoState(const AvoidancePlanningData & data) const; - /** - * @brief check whether the ego is shifted based on shift line. - * @return result. - */ - bool isAvoidanceManeuverRunning(); - - /** - * @brief check whether the ego is in avoidance maneuver based on shift line and target object - * existence. - * @return result. - */ - bool isAvoidancePlanRunning() const; - // ego behavior update /** @@ -245,9 +242,8 @@ class AvoidanceModule : public SceneModuleInterface /** * @brief update main avoidance data for avoidance path generation based on latest planner data. - * @return avoidance data. */ - AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const; + void fillFundamentalData(AvoidancePlanningData & data, DebugData & debug); /** * @brief fill additional data so that the module judges target objects. @@ -256,12 +252,6 @@ class AvoidanceModule : public SceneModuleInterface ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; - /** - * @brief get objects that are driving on adjacent lanes. - * @param left or right lanelets. - */ - ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const; - /** * @brief fill additional data so that the module judges target objects. * @param avoidance data. @@ -338,6 +328,8 @@ class AvoidanceModule : public SceneModuleInterface AvoidLineArray applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_points, DebugData & debug) const; + AvoidLineArray getFillGapShiftLines(const AvoidLineArray & shift_lines) const; + /* * @brief merge negative & positive shift lines. * @param original shift lines. @@ -431,7 +423,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief add new shift line to path shifter if the RTC status is activated. * @param new shift lines. */ - void addShiftLineIfApproved(const AvoidLineArray & point); + void updatePathShifter(const AvoidLineArray & point); /** * @brief add new shift line to path shifter. @@ -481,65 +473,22 @@ class AvoidanceModule : public SceneModuleInterface // safety check - /** - * @brief get shift side adjacent lanes. - * @param path shifter. - * @param forward distance to get. - * @param backward distance to get. - * @return adjacent lanes. - */ - lanelet::ConstLanelets getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const; - - /** - * @brief calculate lateral margin from avoidance velocity for safety check. - * @param target velocity. - */ - double getLateralMarginFromVelocity(const double velocity) const; - - /** - * @brief calculate rss longitudinal distance for safety check. - * @param ego velocity. - * @param object velocity. - * @param option for rss calculation. - * @return rss longitudinal distance. - */ - double getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const; - /** * @brief check avoidance path safety for surround moving objects. - * @param path shifter. * @param avoidance path. * @param debug data. * @return result. */ - bool isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const; + bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const; - /** - * @brief check avoidance path safety for surround moving objects. - * @param avoidance path. - * @param shift side lanes. - * @param debug data. - * @return result. - */ - bool isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, - DebugData & debug) const; - - /** - * @brief check predicted points safety. - * @param predicted points. - * @param future time. - * @param object data. - * @param margin data for debug. - * @return result. - */ - bool isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const; + bool isComfortable(const AvoidLineArray & shift_lines) const + { + return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { + return PathShifter::calcJerkFromLatLonDistance( + line.getRelativeLength(), line.getRelativeLongitudinal(), + helper_.getAvoidanceEgoSpeed()) < helper_.getLateralMaxJerkLimit(); + }); + } // post process @@ -564,6 +513,12 @@ class AvoidanceModule : public SceneModuleInterface unlockNewModuleLaunch(); + if (!path_shifter_.getShiftLines().empty()) { + left_shift_array_.clear(); + right_shift_array_.clear(); + removeRTCStatus(); + } + current_raw_shift_lines_.clear(); registered_raw_shift_lines_.clear(); path_shifter_.setShiftLines(ShiftLineArray{}); @@ -605,7 +560,7 @@ class AvoidanceModule : public SceneModuleInterface helper::avoidance::AvoidanceHelper helper_; - AvoidancePlanningData avoidance_data_; + AvoidancePlanningData avoid_data_; PathShifter path_shifter_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp index e4421459e61fd..9d89138f7cdfe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp @@ -35,17 +35,15 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface AvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; private: std::shared_ptr parameters_; - - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index d43e1d08050e7..dd0e06fffb255 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -35,8 +35,18 @@ namespace behavior_path_planner { +struct MinMaxValue +{ + double min_value; + double max_value; +}; + struct DynamicAvoidanceParameters { + // common + bool enable_debug_info{true}; + bool use_hatched_road_markings{true}; + // obstacle types to avoid bool avoid_car{true}; bool avoid_truck{true}; @@ -48,17 +58,27 @@ struct DynamicAvoidanceParameters bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; + int successive_num_to_exit_dynamic_avoidance_condition{0}; double min_obj_lat_offset_to_ego_path{0.0}; double max_obj_lat_offset_to_ego_path{0.0}; + double min_time_to_start_cut_in{0.0}; + double min_lon_offset_ego_to_cut_in_object{0.0}; + double max_time_from_outside_ego_path_for_cut_out{0.0}; + double min_cut_out_object_lat_vel{0.0}; double max_front_object_angle{0.0}; - double min_crossing_object_vel{0.0}; - double max_crossing_object_angle{0.0}; + double min_overtaking_crossing_object_vel{0.0}; + double max_overtaking_crossing_object_angle{0.0}; + double min_oncoming_crossing_object_vel{0.0}; + double max_oncoming_crossing_object_angle{0.0}; // drivable area generation + std::string polygon_generation_method{}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; + double max_time_for_lat_shift{0.0}; + double lpf_gain_for_lat_avoid_to_offset{0.0}; double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; @@ -77,14 +97,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface { DynamicAvoidanceObject( const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, - const bool arg_is_left, const double arg_time_to_collision) + const bool arg_is_object_on_ego_path, + const std::optional & arg_latest_time_inside_ego_path) : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), shape(predicted_object.shape), vel(arg_vel), lat_vel(arg_lat_vel), - is_left(arg_is_left), - time_to_collision(arg_time_to_collision) + is_object_on_ego_path(arg_is_object_on_ego_path), + latest_time_inside_ego_path(arg_latest_time_inside_ego_path) { for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); @@ -96,27 +117,136 @@ class DynamicAvoidanceModule : public SceneModuleInterface autoware_auto_perception_msgs::msg::Shape shape; double vel; double lat_vel; - bool is_left; - double time_to_collision; + bool is_object_on_ego_path; + std::optional latest_time_inside_ego_path{std::nullopt}; std::vector predicted_paths{}; + + // NOTE: Previous values of the following are used for low-pass filtering. + // Therefore, they has to be initialized as nullopt. + std::optional lon_offset_to_avoid{std::nullopt}; + std::optional lat_offset_to_avoid{std::nullopt}; + bool is_collision_left; + bool should_be_avoided{false}; + + 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) + { + 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; + } }; - struct DynamicAvoidanceObjectCandidate + + struct TargetObjectsManager { - DynamicAvoidanceObject object; - int alive_counter; + TargetObjectsManager(const int arg_max_count, const int arg_min_count) + : max_count_(arg_max_count), min_count_(arg_min_count) + { + } + int max_count_; + int min_count_; - static std::optional getObjectFromUuid( - const std::vector & objects, const std::string & target_uuid) + void initialize() { current_uuids_.clear(); } + void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) { - const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { - return object.object.uuid == target_uuid; - }); + // add/update object + if (object_map_.count(uuid) != 0) { + object_map_.at(uuid) = object; + } else { + object_map_.emplace(uuid, object); + } + + // increase counter + if (counter_map_.count(uuid) != 0) { + counter_map_.at(uuid) = std::min(max_count_ + 1, std::max(1, counter_map_.at(uuid) + 1)); + } else { + counter_map_.emplace(uuid, 1); + } + + // memorize uuid + current_uuids_.push_back(uuid); + } + + void finalize() + { + // 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()) { + not_updated_uuids.push_back(object.first); + } + } + for (const auto & uuid : not_updated_uuids) { + if (counter_map_.count(uuid) != 0) { + counter_map_.at(uuid) = std::max(min_count_ - 1, std::min(-1, counter_map_.at(uuid) - 1)); + } else { + counter_map_.emplace(uuid, -1); + } + } - if (itr == objects.end()) { + // remove objects whose counter is lower than threshold + std::vector obsolete_uuids; + for (const auto & counter : counter_map_) { + if (counter.second < min_count_) { + obsolete_uuids.push_back(counter.first); + } + } + for (const auto & obsolete_uuid : obsolete_uuids) { + counter_map_.erase(obsolete_uuid); + object_map_.erase(obsolete_uuid); + } + } + 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); + } + } + } + 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 *itr; + return object_map_.at(uuid); } + void updateObject( + 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) + { + 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); + } + } + + std::vector current_uuids_; + // NOTE: positive is for meeting entry condition, and negative is for exiting. + std::unordered_map counter_map_; + std::unordered_map object_map_; + }; + + struct DecisionWithReason + { + bool decision; + std::string reason{""}; }; DynamicAvoidanceModule( @@ -124,18 +254,37 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override + { + parameters_ = std::any_cast>(parameters); + } + + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override { - parameters_ = parameters; + updateData(); + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } } bool isExecutionRequested() const override; bool isExecutionReady() const override; - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; BehaviorModuleOutput plan() override; - CandidateOutput planCandidate() const override; BehaviorModuleOutput planWaitingApproval() override; + CandidateOutput planCandidate() const override; void updateData() override; void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override @@ -143,72 +292,68 @@ class DynamicAvoidanceModule : public SceneModuleInterface } private: + struct LatLonOffset + { + const size_t nearest_idx; + const double max_lat_offset; + const double min_lat_offset; + const double max_lon_offset; + const double min_lon_offset; + }; + + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjectsCandidate(); + void updateTargetObjects(); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel) const; - bool willObjectCutOut( - const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const; + 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 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; + 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; + MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const double time_to_collision) const; + MinMaxValue calcMinMaxLateralOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const Polygon2d & obj_points, const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const; std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; - std::optional calcDynamicObstaclePolygon( + std::optional calcEgoPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const; + std::optional calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const; - std::vector - prev_target_objects_candidate_; + void printIgnoreReason(const std::string & obj_uuid, const std::string & reason) + { + const auto reason_text = + "[DynamicAvoidance] Ignore obstacle (%s)" + (reason == "" ? "." : " since " + reason + "."); + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, reason_text.c_str(), obj_uuid.c_str()); + } + std::vector target_objects_; // std::vector prev_target_objects_; std::shared_ptr parameters_; - struct ObjectsVariable - { - void resetCurrentUuids() { current_uuids_.clear(); } - void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); } - void removeCounterUnlessUpdated() - { - std::vector obsolete_uuids; - for (const auto & key_and_value : variable_) { - if ( - std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) == - current_uuids_.end()) { - obsolete_uuids.push_back(key_and_value.first); - } - } - - for (const auto & obsolete_uuid : obsolete_uuids) { - variable_.erase(obsolete_uuid); - } - } - - std::optional get(const std::string & uuid) const - { - if (variable_.count(uuid) != 0) { - return variable_.at(uuid); - } - return std::nullopt; - } - void update(const std::string & uuid, const double new_variable) - { - if (variable_.count(uuid) != 0) { - variable_.at(uuid) = new_variable; - } else { - variable_.emplace(uuid, new_variable); - } - } - - std::unordered_map variable_; - std::vector current_uuids_; - }; - mutable ObjectsVariable prev_objects_min_bound_lat_offset_; + TargetObjectsManager target_objects_manager_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index d2b1deb36bd30..10a0d055a5a77 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -34,9 +34,9 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface DynamicAvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_); } @@ -44,7 +44,6 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface private: std::shared_ptr parameters_; - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 178ae5bc79e2b..8d9c8e147f532 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -80,10 +80,21 @@ struct PUllOverStatus bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; - std::optional stop_pose{}; bool is_ready{false}; }; +struct FreespacePlannerDebugData +{ + bool is_planning{false}; + size_t current_goal_idx{0}; + size_t num_goal_candidates{0}; +}; + +struct GoalPlannerDebugData +{ + FreespacePlannerDebugData freespace_planner{}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -92,16 +103,17 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } - BehaviorModuleOutput run() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override; bool isExecutionRequested() const override; bool isExecutionReady() const override; - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; void processOnEntry() override; @@ -116,6 +128,12 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; }; private: + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + PUllOverStatus status_; std::shared_ptr parameters_; @@ -170,6 +188,9 @@ class GoalPlannerModule : public SceneModuleInterface rclcpp::TimerBase::SharedPtr freespace_parking_timer_; rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; + // debug + mutable GoalPlannerDebugData debug_data_; + // collision check void initializeOccupancyGridMap(); void updateOccupancyGrid(); @@ -195,7 +216,7 @@ class GoalPlannerModule : public SceneModuleInterface std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); bool hasFinishedGoalPlanner(); - bool isOnGoal() const; + bool isOnModifiedGoal() const; double calcModuleRequestLength() const; void resetStatus(); bool needPathUpdate(const double path_update_duration) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index d836f73df22bc..c410999b2aaa9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -33,17 +33,21 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface GoalPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; + bool isSimultaneousExecutableAsApprovedModule() const override; + + bool isSimultaneousExecutableAsCandidateModule() const override; + private: std::shared_ptr parameters_; - std::vector> registered_modules_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 133998b493567..6ca06ff21c4a9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -137,7 +137,11 @@ class LaneChangeBase virtual void updateSpecialData() {} - virtual void insertStopPoint([[maybe_unused]] PathWithLaneId & path) {} + virtual void insertStopPoint( + [[maybe_unused]] const lanelet::ConstLanelets & lanelets, + [[maybe_unused]] PathWithLaneId & path) + { + } const LaneChangeStatus & getLaneChangeStatus() const { return status_; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 34ded5c976e3b..b7f84987e0499 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -73,7 +73,8 @@ class LaneChangeInterface : public SceneModuleInterface bool isExecutionReady() const override; - ModuleStatus updateState() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; void updateData() override; @@ -87,7 +88,7 @@ class LaneChangeInterface : public SceneModuleInterface void acceptVisitor(const std::shared_ptr & visitor) const override; - void updateModuleParams(const std::shared_ptr & parameters); + void updateModuleParams(const std::any & parameters) override; void setData(const std::shared_ptr & data) override; @@ -96,11 +97,36 @@ class LaneChangeInterface : public SceneModuleInterface TurnSignalInfo getCurrentTurnSignalInfo( const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info); + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override + { + updateData(); + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } + } + protected: std::shared_ptr parameters_; std::unique_ptr module_type_; + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + void resetPathIfAbort(); void resetLaneChangeModule(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index 9dabd3d0004fb..cf618b755b094 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -37,15 +37,13 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const Direction direction, const LaneChangeModuleType type); - std::shared_ptr createNewSceneModuleInstance() override; + std::unique_ptr createNewSceneModuleInstance() override; void updateModuleParams(const std::vector & parameters) override; protected: std::shared_ptr parameters_; - std::vector> registered_modules_; - Direction direction_; LaneChangeModuleType type_; @@ -57,7 +55,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager AvoidanceByLaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override; + std::unique_ptr createNewSceneModuleInstance() override; // void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index bebda4eb7f353..5d13507f5206c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -26,10 +26,10 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -62,7 +62,7 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) override; - void insertStopPoint(PathWithLaneId & path) override; + void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; PathWithLaneId getReferencePath() const override; @@ -104,6 +104,10 @@ class NormalLaneChange : public LaneChangeBase int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + std::vector sampleLongitudinalAccValues( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + double calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; @@ -136,9 +140,13 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const; + LaneChangeTargetObjectIndices filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const; + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 98dd98c707a23..3405ae446615d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -16,12 +16,12 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/module_status.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/utils.hpp" #include #include +#include #include #include #include @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -65,6 +66,14 @@ using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; using PlanResult = PathWithLaneId::SharedPtr; +enum class ModuleStatus { + IDLE = 0, + RUNNING = 1, + WAITING_APPROVAL = 2, + SUCCESS = 3, + FAILURE = 4, +}; + class SceneModuleInterface { public: @@ -74,9 +83,6 @@ class SceneModuleInterface : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, - is_waiting_approval_{false}, - is_locked_new_module_launch_{false}, - current_state_{ModuleStatus::IDLE}, rtc_interface_ptr_map_(rtc_interface_ptr_map), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) @@ -88,23 +94,24 @@ class SceneModuleInterface virtual ~SceneModuleInterface() = default; - /** - * @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() = 0; + virtual void updateModuleParams(const std::any & parameters) = 0; - /** - * @brief Set the current_state_ based on updateState output. - */ - virtual void updateCurrentState() { current_state_ = updateState(); } + virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; /** - * @brief If the module plan customized reference path while waiting approval, it should output - * SUCCESS. Otherwise, it should output FAILURE to check execution request of next module. + * @brief Set the current_state_ based on updateState output. */ - virtual ModuleStatus getNodeStatusWhileWaitingApproval() const { return ModuleStatus::FAILURE; } + 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) @@ -116,36 +123,6 @@ class SceneModuleInterface */ virtual bool isExecutionReady() const = 0; - /** - * @brief Calculate path. This function is called with the plan is approved. - */ - virtual BehaviorModuleOutput plan() = 0; - - /** - * @brief Calculate path under waiting_approval condition. - * The default implementation is just to return the reference path. - */ - virtual BehaviorModuleOutput planWaitingApproval() - { - BehaviorModuleOutput out; - out.path = utils::generateCenterLinePath(planner_data_); - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - - // for new architecture - const auto lanes = utils::getLaneletsFromPath(*out.path, planner_data_->route_handler); - const auto drivable_lanes = utils::generateDrivableLanes(lanes); - out.drivable_area_info.drivable_lanes = - getNonOverlappingExpandedLanes(*out.path, drivable_lanes); - - return out; - } - - /** - * @brief Get candidate path. This information is used for external judgement. - */ - virtual CandidateOutput planCandidate() const = 0; - /** * @brief update data for planning. Note that the call of this function does not mean * that the module executed. It should only updates the data necessary for @@ -160,19 +137,7 @@ class SceneModuleInterface virtual BehaviorModuleOutput run() { updateData(); - - if (!isWaitingApproval()) { - return plan(); - } - - // module is waiting approval. Check it. - if (isActivated()) { - RCLCPP_DEBUG(logger_, "Was waiting approval, and now approved. Do plan()."); - return plan(); - } else { - RCLCPP_DEBUG(logger_, "keep waiting approval... Do planCandidate()."); - return planWaitingApproval(); - } + return isWaitingApproval() ? planWaitingApproval() : plan(); } /** @@ -182,15 +147,11 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); - current_state_ = ModuleStatus::IDLE; - stop_reason_ = StopReason(); processOnEntry(); } - virtual void processOnEntry() {} - /** * @brief Called when the module exit from RUNNING. */ @@ -198,10 +159,11 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); - current_state_ = ModuleStatus::SUCCESS; clearWaitingApproval(); removeRTCStatus(); + publishRTCStatus(); unlockNewModuleLaunch(); + unlockOutputPath(); steering_factor_interface_ptr_->clearSteeringFactors(); stop_reason_ = StopReason(); @@ -209,8 +171,6 @@ class SceneModuleInterface processOnExit(); } - virtual void processOnExit() {} - /** * @brief Publish status if the module is requested to run */ @@ -223,24 +183,6 @@ class SceneModuleInterface } } - /** - * @brief Return true if the activation command is received from the RTC interface. - * If no RTC interface is registered, return true. - */ - bool isActivated() - { - if (rtc_interface_ptr_map_.empty()) { - return true; - } - - for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { - if (itr->second->isRegistered(uuid_map_.at(itr->first))) { - return itr->second->isActivated(uuid_map_.at(itr->first)); - } - } - return false; - } - void publishSteeringFactor() { if (!steering_factor_interface_ptr_) { @@ -280,13 +222,16 @@ class SceneModuleInterface */ virtual void setData(const std::shared_ptr & data) { planner_data_ = data; } - bool isWaitingApproval() const { return is_waiting_approval_; } + void lockOutputPath() { is_locked_output_path_ = true; } - bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } + void unlockOutputPath() { is_locked_output_path_ = false; } - void resetPathCandidate() { path_candidate_.reset(); } + bool isWaitingApproval() const + { + return is_waiting_approval_ || current_state_ == ModuleStatus::WAITING_APPROVAL; + } - void resetPathReference() { path_reference_.reset(); } + bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } PlanResult getPathCandidate() const { return path_candidate_; } @@ -304,8 +249,6 @@ class SceneModuleInterface StopReason getStopReason() const { return stop_reason_; } - virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; - std::string name() const { return name_; } boost::optional getStopPose() const @@ -345,57 +288,114 @@ class SceneModuleInterface dead_pose_ = boost::none; } - void setDrivableLanes(const std::vector & drivable_lanes) + rclcpp::Logger getLogger() const { return logger_; } + +private: + bool existRegisteredRequest() const { - drivable_lanes_marker_ = - marker_utils::createDrivableLanesMarkerArray(drivable_lanes, "drivable_lanes"); + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)); }); } - rclcpp::Logger getLogger() const { return logger_; } - - void setIsSimultaneousExecutableAsApprovedModule(const bool enable) + bool existApprovedRequest() const { - is_simultaneously_executable_as_approved_module_ = enable; + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { + return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && + rtc.second->isActivated(uuid_map_.at(rtc.first)); + }); } - bool isSimultaneousExecutableAsApprovedModule() const + bool existNotApprovedRequest() const { - return is_simultaneously_executable_as_approved_module_; + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { + return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && + !rtc.second->isActivated(uuid_map_.at(rtc.first)); + }); } - void setIsSimultaneousExecutableAsCandidateModule(const bool enable) + bool canTransitWaitingApprovalState() const { - is_simultaneously_executable_as_candidate_module_ = enable; + if (!existRegisteredRequest()) { + return false; + } + return existNotApprovedRequest(); } - bool isSimultaneousExecutableAsCandidateModule() const + bool canTransitWaitingApprovalToRunningState() const { - return is_simultaneously_executable_as_candidate_module_; + if (!existRegisteredRequest()) { + return true; + } + return existApprovedRequest(); } -private: std::string name_; rclcpp::Logger logger_; BehaviorModuleOutput previous_module_output_; + StopReason stop_reason_; + + bool is_simultaneously_executable_as_approved_module_{false}; + + bool is_simultaneously_executable_as_candidate_module_{false}; + + bool is_locked_new_module_launch_{false}; + + bool is_locked_output_path_{false}; + protected: - // TODO(murooka) Remove this function when BT-based architecture will be removed - std::unordered_map> createRTCInterfaceMap( - rclcpp::Node & node, const std::string & name, const std::vector & rtc_types) + /** + * @brief State transition condition ANY -> SUCCESS + */ + virtual bool canTransitSuccessState() = 0; + + /** + * @brief State transition condition ANY -> FAILURE + */ + virtual bool canTransitFailureState() = 0; + + /** + * @brief State transition condition IDLE -> RUNNING + */ + virtual bool canTransitIdleToRunningState() = 0; + + /** + * @brief Get candidate path. This information is used for external judgement. + */ + virtual CandidateOutput planCandidate() const = 0; + + /** + * @brief Calculate path. This function is called with the plan is approved. + */ + virtual BehaviorModuleOutput plan() = 0; + + /** + * @brief Calculate path under waiting_approval condition. + * The default implementation is just to return the reference path. + */ + virtual BehaviorModuleOutput planWaitingApproval() { - std::unordered_map> rtc_interface_ptr_map; - for (const auto & rtc_type : rtc_types) { - const auto snake_case_name = utils::convertToSnakeCase(name); - const auto rtc_interface_name = - 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)); - } - return rtc_interface_ptr_map; + path_candidate_ = std::make_shared(planCandidate().path_candidate); + path_reference_ = getPreviousModuleOutput().reference_path; + + return getPreviousModuleOutput(); } + /** + * @brief Module unique entry process. + */ + virtual void processOnEntry() {} + + /** + * @brief Module unique exit process. + */ + virtual void processOnExit() {} + virtual void updateRTCStatus(const double start_distance, const double finish_distance) { for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -407,6 +407,80 @@ 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. + */ + bool isActivated() + { + if (rtc_interface_ptr_map_.empty()) { + return true; + } + + if (!existRegisteredRequest()) { + return false; + } + return existApprovedRequest(); + } + void removeRTCStatus() { for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -433,8 +507,16 @@ class SceneModuleInterface stop_reason_.stop_factors.push_back(stop_factor); } + void setDrivableLanes(const std::vector & drivable_lanes) + { + drivable_lanes_marker_ = + marker_utils::createDrivableLanesMarkerArray(drivable_lanes, "drivable_lanes"); + } + BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + bool isOutputPathLocked() const { return is_locked_output_path_; } + void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; } @@ -443,6 +525,10 @@ class SceneModuleInterface void clearWaitingApproval() { is_waiting_approval_ = false; } + void resetPathCandidate() { path_candidate_.reset(); } + + void resetPathReference() { path_reference_.reset(); } + geometry_msgs::msg::Point getEgoPosition() const { return planner_data_->self_odometry->pose.pose.position; @@ -460,27 +546,11 @@ class SceneModuleInterface return std::abs(planner_data_->self_odometry->twist.twist.linear.x); } - std::vector getNonOverlappingExpandedLanes( - PathWithLaneId & path, const std::vector & lanes) const - { - const auto & dp = planner_data_->drivable_area_expansion_parameters; - - const auto shorten_lanes = utils::cutOverlappedLanes(path, 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); - return expanded_lanes; - } - - bool is_simultaneously_executable_as_approved_module_{false}; - bool is_simultaneously_executable_as_candidate_module_{false}; - rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - bool is_waiting_approval_; - bool is_locked_new_module_launch_; + bool is_waiting_approval_{false}; std::unordered_map uuid_map_; @@ -489,8 +559,6 @@ class SceneModuleInterface ModuleStatus current_state_{ModuleStatus::IDLE}; - StopReason stop_reason_; - std::unordered_map> rtc_interface_ptr_map_; std::unique_ptr steering_factor_interface_ptr_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 0c6ec17e94a6a..cb47c33b6901c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -38,6 +38,7 @@ using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; +using SceneModuleObserver = std::weak_ptr; class SceneModuleManagerInterface { @@ -73,56 +74,46 @@ class SceneModuleManagerInterface virtual ~SceneModuleManagerInterface() = default; - SceneModulePtr getNewModule() + void updateIdleModuleInstance() { - if (idling_module_ptr_ != nullptr) { - idling_module_ptr_->onEntry(); - return idling_module_ptr_; + if (idle_module_ptr_) { + idle_module_ptr_->onEntry(); + return; } - idling_module_ptr_ = createNewSceneModuleInstance(); - return idling_module_ptr_; + idle_module_ptr_ = createNewSceneModuleInstance(); } - bool isExecutionRequested( - const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) const + bool isExecutionRequested(const BehaviorModuleOutput & previous_module_output) const { - module_ptr->setData(planner_data_); - module_ptr->setPreviousModuleOutput(previous_module_output); - module_ptr->updateData(); + idle_module_ptr_->setData(planner_data_); + idle_module_ptr_->setPreviousModuleOutput(previous_module_output); + idle_module_ptr_->updateData(); - return module_ptr->isExecutionRequested(); + return idle_module_ptr_->isExecutionRequested(); } void registerNewModule( - const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) + const SceneModuleObserver & observer, const BehaviorModuleOutput & previous_module_output) { - module_ptr->setIsSimultaneousExecutableAsApprovedModule( - enable_simultaneous_execution_as_approved_module_); - module_ptr->setIsSimultaneousExecutableAsCandidateModule( - enable_simultaneous_execution_as_candidate_module_); - module_ptr->setData(planner_data_); - module_ptr->setPreviousModuleOutput(previous_module_output); - module_ptr->onEntry(); - - registered_modules_.push_back(module_ptr); - } - - void deleteModules(SceneModulePtr & module_ptr) - { - module_ptr->onExit(); - module_ptr->publishRTCStatus(); + if (observer.expired()) { + return; + } - const auto itr = std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr); + observer.lock()->setData(planner_data_); + observer.lock()->setPreviousModuleOutput(previous_module_output); + observer.lock()->onEntry(); - if (itr != registered_modules_.end()) { - registered_modules_.erase(itr); - } + observers_.push_back(observer); + } - module_ptr.reset(); - idling_module_ptr_.reset(); + void updateObserver() + { + const auto itr = std::remove_if( + observers_.begin(), observers_.end(), + [](const auto & observer) { return observer.expired(); }); - pub_debug_marker_->publish(MarkerArray{}); + observers_.erase(itr, observers_.end()); } void publishVirtualWall() const @@ -134,32 +125,36 @@ class SceneModuleManagerInterface const auto marker_offset = std::numeric_limits::max(); uint32_t marker_id = marker_offset; - for (const auto & m : registered_modules_) { - const auto opt_stop_pose = m->getStopPose(); + for (const auto & m : observers_) { + if (m.expired()) { + continue; + } + + const auto opt_stop_pose = m.lock()->getStopPose(); if (!!opt_stop_pose) { const auto virtual_wall = createStopVirtualWallMarker( - opt_stop_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_stop_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto opt_slow_pose = m->getSlowPose(); + const auto opt_slow_pose = m.lock()->getSlowPose(); if (!!opt_slow_pose) { const auto virtual_wall = createSlowDownVirtualWallMarker( - opt_slow_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_slow_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto opt_dead_pose = m->getDeadPose(); + const auto opt_dead_pose = m.lock()->getDeadPose(); if (!!opt_dead_pose) { const auto virtual_wall = createDeadLineVirtualWallMarker( - opt_dead_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_dead_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto module_specific_wall = m->getModuleVirtualWall(); + const auto module_specific_wall = m.lock()->getModuleVirtualWall(); appendMarkerArray(module_specific_wall, &markers); - m->resetWallPoses(); + m.lock()->resetWallPoses(); } pub_virtual_wall_->publish(markers); @@ -176,18 +171,22 @@ class SceneModuleManagerInterface const auto marker_offset = std::numeric_limits::max(); uint32_t marker_id = marker_offset; - for (const auto & m : registered_modules_) { - for (auto & marker : m->getInfoMarkers().markers) { + for (const auto & m : observers_) { + if (m.expired()) { + continue; + } + + for (auto & marker : m.lock()->getInfoMarkers().markers) { marker.id += marker_id; info_markers.markers.push_back(marker); } - for (auto & marker : m->getDebugMarkers().markers) { + for (auto & marker : m.lock()->getDebugMarkers().markers) { marker.id += marker_id; debug_markers.markers.push_back(marker); } - for (auto & marker : m->getDrivableLanesMarkers().markers) { + for (auto & marker : m.lock()->getDrivableLanesMarkers().markers) { marker.id += marker_id; drivable_lanes_markers.markers.push_back(marker); } @@ -195,10 +194,10 @@ class SceneModuleManagerInterface marker_id += marker_offset; } - if (registered_modules_.empty() && idling_module_ptr_ != nullptr) { - appendMarkerArray(idling_module_ptr_->getInfoMarkers(), &info_markers); - appendMarkerArray(idling_module_ptr_->getDebugMarkers(), &debug_markers); - appendMarkerArray(idling_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers); + if (observers_.empty() && idle_module_ptr_ != nullptr) { + appendMarkerArray(idle_module_ptr_->getInfoMarkers(), &info_markers); + appendMarkerArray(idle_module_ptr_->getDebugMarkers(), &debug_markers); + appendMarkerArray(idle_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers); } pub_info_marker_->publish(info_markers); @@ -208,50 +207,40 @@ class SceneModuleManagerInterface bool exist(const SceneModulePtr & module_ptr) const { - return std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr) != - registered_modules_.end(); + return std::any_of(observers_.begin(), observers_.end(), [&](const auto & observer) { + return !observer.expired() && observer.lock() == module_ptr; + }); } - bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; } + bool canLaunchNewModule() const { return observers_.size() < max_module_num_; } - bool isSimultaneousExecutableAsApprovedModule() const + virtual bool isSimultaneousExecutableAsApprovedModule() const { - if (registered_modules_.empty()) { - return enable_simultaneous_execution_as_approved_module_; - } - - return std::all_of( - registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { - return module->isSimultaneousExecutableAsApprovedModule(); - }); + return enable_simultaneous_execution_as_approved_module_; } - bool isSimultaneousExecutableAsCandidateModule() const + virtual bool isSimultaneousExecutableAsCandidateModule() const { - if (registered_modules_.empty()) { - return enable_simultaneous_execution_as_candidate_module_; - } - - return std::all_of( - registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { - return module->isSimultaneousExecutableAsCandidateModule(); - }); + return enable_simultaneous_execution_as_candidate_module_; } void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() { - std::for_each(registered_modules_.begin(), registered_modules_.end(), [](const auto & m) { - m->onExit(); - m->publishRTCStatus(); + std::for_each(observers_.begin(), observers_.end(), [](const auto & observer) { + if (!observer.expired()) { + observer.lock()->onExit(); + observer.lock()->publishRTCStatus(); + } }); - registered_modules_.clear(); - if (idling_module_ptr_ != nullptr) { - idling_module_ptr_->onExit(); - idling_module_ptr_->publishRTCStatus(); - idling_module_ptr_.reset(); + observers_.clear(); + + if (idle_module_ptr_ != nullptr) { + idle_module_ptr_->onExit(); + idle_module_ptr_->publishRTCStatus(); + idle_module_ptr_.reset(); } pub_debug_marker_->publish(MarkerArray{}); @@ -259,14 +248,16 @@ class SceneModuleManagerInterface size_t getPriority() const { return priority_; } - std::string getModuleName() const { return name_; } + std::string name() const { return name_; } + + std::vector getSceneModuleObservers() { return observers_; } - std::vector getSceneModules() { return registered_modules_; } + std::shared_ptr getIdleModule() { return std::move(idle_module_ptr_); } virtual void updateModuleParams(const std::vector & parameters) = 0; protected: - virtual std::shared_ptr createNewSceneModuleInstance() = 0; + virtual std::unique_ptr createNewSceneModuleInstance() = 0; rclcpp::Node * node_; @@ -286,9 +277,9 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; - std::vector registered_modules_; + std::vector observers_; - SceneModulePtr idling_module_ptr_; + std::unique_ptr idle_module_ptr_; std::unordered_map> rtc_interface_ptr_map_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp index f35f497df0e4a..b74be38c0faf9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp @@ -34,17 +34,15 @@ class SideShiftModuleManager : public SceneModuleManagerInterface SideShiftModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; private: std::shared_ptr parameters_; - - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 8118d05ef225d..7c04cab5c081f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -43,15 +43,15 @@ class SideShiftModule : public SceneModuleInterface SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map); bool isExecutionRequested() const override; bool isExecutionReady() const override; bool isReadyForNextRequest( const double & min_request_time_sec, bool override_requests = false) const noexcept; - ModuleStatus updateState() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; void updateData() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -60,9 +60,9 @@ class SideShiftModule : public SceneModuleInterface void setParameters(const std::shared_ptr & parameters); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } void acceptVisitor( @@ -70,7 +70,31 @@ class SideShiftModule : public SceneModuleInterface { } + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override + { + updateData(); + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } + } + private: + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; void initVariables(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp index 103e7dc375233..3ee376f4111f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp @@ -33,17 +33,19 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface StartPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; + bool isSimultaneousExecutableAsApprovedModule() const override; + + bool isSimultaneousExecutableAsCandidateModule() const override; + private: std::shared_ptr parameters_; - - std::vector> registered_modules_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 205b3c6d4db46..2e8c9e57f8842 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" @@ -51,12 +52,9 @@ struct PullOutStatus size_t current_path_idx{0}; PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; - lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets pull_out_lanes{}; - std::vector lanes{}; - std::vector lane_follow_lane_ids{}; - std::vector pull_out_lane_ids{}; - bool is_safe{false}; + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool back_finished{false}; Pose pull_out_start_pose{}; @@ -71,20 +69,22 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::any & parameters) override { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } - BehaviorModuleOutput run() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override; bool isExecutionRequested() const override; bool isExecutionReady() const override; - ModuleStatus updateState() override; - ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; } + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; + void processOnExit() override; void setParameters(const std::shared_ptr & parameters) @@ -98,20 +98,17 @@ class StartPlannerModule : public SceneModuleInterface { } - // set is_simultaneously_executable_ as false when backward driving. - // keep initial value to return it after finishing backward driving. - bool initial_value_simultaneously_executable_as_approved_module_; - bool initial_value_simultaneously_executable_as_candidate_module_; - void setInitialIsSimultaneousExecutableAsApprovedModule(const bool is_simultaneously_executable) - { - initial_value_simultaneously_executable_as_approved_module_ = is_simultaneously_executable; - }; - void setInitialIsSimultaneousExecutableAsCandidateModule(const bool is_simultaneously_executable) - { - initial_value_simultaneously_executable_as_candidate_module_ = is_simultaneously_executable; - }; + // Condition to disable simultaneous execution + bool isBackFinished() const { return status_.back_finished; } + bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + bool canTransitSuccessState() override { return false; } + + bool canTransitFailureState() override { return false; } + + bool canTransitIdleToRunningState() override { return false; } + std::shared_ptr parameters_; vehicle_info_util::VehicleInfo vehicle_info_; @@ -123,9 +120,16 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; std::unique_ptr last_approved_pose_; - mutable bool has_received_new_route_{false}; - std::shared_ptr getCurrentPlanner() const; + // generate freespace pull out paths in a separate thread + std::unique_ptr freespace_planner_; + rclcpp::TimerBase::SharedPtr freespace_planner_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_; + // TODO(kosuke55) + // Currently, we only do lock when updating a member of status_. + // However, we need to ensure that the value does not change when referring to it. + std::mutex mutex_; + PathWithLaneId getFullPath() const; std::vector searchPullOutStartPoses(); @@ -140,6 +144,8 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; + lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; + std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, @@ -147,13 +153,19 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedPullOut() const; void checkBackFinished(); bool isStopped(); + bool isStuck(); bool hasFinishedCurrentPath(); + void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. bool IsGoalBehindOfEgoInSameRouteSegment() const; - // generate BehaviorPathOutput with stopping path. - BehaviorModuleOutput generateStopOutput() const; + // generate BehaviorPathOutput with stopping path and update status + BehaviorModuleOutput generateStopOutput(); + + // freespace planner + void onFreespacePlannerTimer(); + bool planFreespacePath(); void setDebugData() const; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 3c01ad20bcebd..1f47b804e185b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,6 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -46,6 +48,8 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +using marker_utils::CollisionCheckDebug; + struct ObjectParameter { bool is_target{false}; @@ -76,12 +80,6 @@ struct AvoidanceParameters // computational cost for latter modules. double resample_interval_for_output = 3.0; - // lanelet expand length for right side to find avoidance target vehicles - double detection_area_right_expand_dist = 0.0; - - // lanelet expand length for left side to find avoidance target vehicles - double detection_area_left_expand_dist = 1.0; - // enable avoidance to be perform only in lane with same direction bool use_adjacent_lane{true}; @@ -89,15 +87,12 @@ struct AvoidanceParameters // to use this, enable_avoidance_over_same_direction need to be set to true. bool use_opposite_lane{true}; - // enable update path when if detected objects on planner data is gone. - bool enable_update_path_when_object_is_gone{false}; + // if this param is true, it reverts avoidance path when the path is no longer needed. + bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle bool enable_force_avoidance_for_stopped_vehicle{false}; - // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver - bool enable_safety_check{false}; - // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -113,8 +108,11 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; - // constrains - bool use_constraints_for_decel{false}; + // // constrains + // bool use_constraints_for_decel{false}; + + // // policy + // bool use_relaxed_margin_immediately{false}; // max deceleration for double max_deceleration; @@ -177,27 +175,27 @@ struct AvoidanceParameters // for longitudinal direction double longitudinal_collision_margin_time; - // find adjacent lane vehicles - double safety_check_backward_distance; - - // minimum longitudinal margin for vehicles in adjacent lane - double safety_check_min_longitudinal_margin; + // parameters for safety check area + bool enable_safety_check{false}; + bool check_current_lane{false}; + bool check_shift_side_lane{false}; + bool check_other_side_lane{false}; - // safety check time horizon - double safety_check_time_horizon; + // parameters for safety check target. + bool check_unavoidable_object{false}; + bool check_other_object{false}; - // use in RSS calculation - double safety_check_idling_time; + // parameters for collision check. + bool check_all_predicted_path{false}; + double safety_check_time_horizon{0.0}; + double safety_check_time_resolution{0.0}; - // use in RSS calculation - double safety_check_accel_for_rss; + // find adjacent lane vehicles + double safety_check_backward_distance; // transit hysteresis (unsafe to safe) double safety_check_hysteresis_factor; - // don't output new candidate path if the offset between ego and path is larger than this. - double safety_check_ego_offset; - // keep target velocity in yield maneuver double yield_velocity; @@ -222,9 +220,16 @@ struct AvoidanceParameters // nominal avoidance sped double nominal_avoidance_speed; + // module try to return original path to keep this distance from edge point of the path. + double remain_buffer_distance; + + // The margin is configured so that the generated avoidance trajectory does not come near to the + // road shoulder. + double soft_road_shoulder_margin{1.0}; + // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. - double road_shoulder_safety_margin{1.0}; + double hard_road_shoulder_margin{1.0}; // Even if the obstacle is very large, it will not avoid more than this length for right direction double max_right_shift_length; @@ -268,6 +273,15 @@ struct AvoidanceParameters // For shift line generation process. Remove sharp(=jerky) shift line. double sharp_shift_filter_threshold; + // policy + bool use_shorten_margin_immediately{false}; + + // policy + std::string policy_deceleration{"best_effort"}; + + // policy + std::string policy_lateral_margin{"best_effort"}; + // target velocity matrix std::vector velocity_map; @@ -280,15 +294,12 @@ struct AvoidanceParameters // Maximum lateral acceleration limitation map. std::vector lateral_max_accel_map; - // target velocity matrix - std::vector target_velocity_matrix; - - // matrix col size - size_t col_size; - // parameters depend on object class std::unordered_map object_parameters; + // rss parameters + utils::path_safety_checker::RSSparams rss_params; + // clip left and right bounds for objects bool enable_bound_clipping{false}; @@ -465,6 +476,8 @@ struct AvoidancePlanningData bool safe{false}; + bool comfortable{false}; + bool avoid_required{false}; bool yield_required{false}; @@ -498,35 +511,15 @@ struct ShiftLineData std::vector> shift_line_history; }; -/* - * Data struct for longitudinal margin - */ -struct MarginData -{ - Pose pose{}; - - bool enough_lateral_margin{true}; - - double longitudinal_distance{std::numeric_limits::max()}; - - double longitudinal_margin{std::numeric_limits::lowest()}; - - double vehicle_width; - - double base_link2front; - - double base_link2rear; -}; -using MarginDataArray = std::vector; - /* * Debug information for marker array */ struct DebugData { - std::shared_ptr expanded_lanelets; std::shared_ptr current_lanelets; + geometry_msgs::msg::Polygon detection_area; + lanelet::ConstLineStrings3d bounds; AvoidLineArray current_shift_lines; // in path shifter @@ -561,14 +554,9 @@ struct DebugData // shift path std::vector proposed_spline_shift; - bool exist_adjacent_objects{false}; - // future pose PathWithLaneId path_with_planned_velocity; - // margin - MarginDataArray margin_data_array; - // avoidance require objects ObjectDataArray unavoidable_objects; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 69803e4cc4dec..6b7830147ebf9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -17,14 +17,22 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include #include #include +#include #include namespace behavior_path_planner::utils::avoidance { using behavior_path_planner::PlannerData; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; bool isOnRight(const ObjectData & obj); @@ -79,11 +87,19 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + 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); + lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset); @@ -139,6 +155,18 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters); + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift); + +std::pair separateObjectsByPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const AvoidancePlanningData & data, const std::shared_ptr & parameters, + DebugData & debug); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index eadfb72a42afa..53ef91473fe24 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -40,7 +40,7 @@ void expandDrivableArea( /// @param[in] expansion_polygons polygons to add to the drivable area /// @return expanded drivable area polygon polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multipolygon_t & expansion_polygons); + const PathWithLaneId & path, const multi_polygon_t & expansion_polygons); /// @brief Update the drivable area of the given path with the given polygon /// @details this function splits the polygon into a left and right bound and sets it in the path diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp index 33bccf90ffbe8..70cc8a8bc5925 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp @@ -36,7 +36,7 @@ namespace drivable_area_expansion /// @return distance limit double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multilinestring_t & limit_lines); + const multi_linestring_t & limit_lines); /// @brief Calculate the distance limit required for the polygon to not cross the limit polygons. /// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and @@ -47,7 +47,7 @@ double calculateDistanceLimit( /// @return distance limit double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multipolygon_t & limit_polygons); + const multi_polygon_t & limit_polygons); /// @brief Create a polygon from a base line with a given expansion distance /// @param[in] base_ls base linestring from which the polygon is created @@ -64,9 +64,9 @@ polygon_t createExpansionPolygon( /// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area /// @param[in] params expansion parameters /// @return expansion polygons -multipolygon_t createExpansionPolygons( - const PathWithLaneId & path, const multipolygon_t & path_footprints, - const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines, +multi_polygon_t createExpansionPolygons( + const PathWithLaneId & path, const multi_polygon_t & path_footprints, + const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, const DrivableAreaExpansionParameters & params); /// @brief Create polygons for the area where the drivable area should be expanded @@ -76,9 +76,9 @@ multipolygon_t createExpansionPolygons( /// @param[in] predicted_paths polygons of the dynamic objects' predicted paths /// @param[in] params expansion parameters /// @return expansion polygons -multipolygon_t createExpansionLaneletPolygons( +multi_polygon_t createExpansionLaneletPolygons( const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multipolygon_t & path_footprints, const multipolygon_t & predicted_paths, + const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 25f061affaa48..e5e1c76f2521f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -55,7 +55,7 @@ polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -multipolygon_t createObjectFootprints( +multi_polygon_t createObjectFootprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); @@ -63,7 +63,7 @@ multipolygon_t createObjectFootprints( /// @param[in] path the path for which to create a footprint /// @param[in] params expansion parameters defining how to create the footprint /// @return footprint polygons of the path -multipolygon_t createPathFootprints( +multi_polygon_t createPathFootprints( const PathWithLaneId & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 33c4cc8a0eff3..4524bd2be2299 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -28,7 +28,7 @@ namespace drivable_area_expansion /// @param[in] lanelet_map lanelet map /// @param[in] uncrossable_types types that cannot be crossed /// @return the uncrossable linestrings -multilinestring_t extractUncrossableLines( +multi_linestring_t extractUncrossableLines( const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types); /// @brief Determine if the given linestring has one of the given types diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index e56ef4961589d..daabfa97598fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -30,13 +30,13 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using point_t = tier4_autoware_utils::Point2d; -using multipoint_t = tier4_autoware_utils::MultiPoint2d; +using multi_point_t = tier4_autoware_utils::MultiPoint2d; using polygon_t = tier4_autoware_utils::Polygon2d; using ring_t = tier4_autoware_utils::LinearRing2d; -using multipolygon_t = tier4_autoware_utils::MultiPolygon2d; +using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; using segment_t = tier4_autoware_utils::Segment2d; using linestring_t = tier4_autoware_utils::LineString2d; -using multilinestring_t = tier4_autoware_utils::MultiLineString2d; +using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; struct PointDistance { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index 1378be5461a64..b698892907789 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase std::unique_ptr planner_; double velocity_; bool use_back_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f4a3a2bc5a88b..f87944641f59b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -68,6 +68,7 @@ struct GoalPlannerParameters // object recognition bool use_object_recognition; double object_recognition_collision_check_margin; + double object_recognition_collision_check_max_extra_stopping_margin; // pull over general params double pull_over_velocity; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index be6e1c0046449..d564dcaa19e29 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -44,6 +44,7 @@ class GoalSearcher : public GoalSearcherBase bool checkCollision(const Pose & pose) const; bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; + BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index b1f548425d48f..3dde96e813aad 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -51,6 +51,11 @@ PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); +bool isAllowedGoalModification( + const std::shared_ptr & route_handler, const bool left_side_parking); +bool checkOriginalGoalIsInShoulder( + const std::shared_ptr & route_handler, const bool left_side_parking); + // debug MarkerArray createPullOverAreaMarkerArray( const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 88ddeba83b8c8..0556a780467c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -15,7 +15,8 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "lanelet2_core/geometry/Lanelet.h" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" @@ -75,6 +76,10 @@ struct LaneChangeParameters bool check_motorcycle{true}; // check object motorbike bool check_pedestrian{true}; // check object pedestrian + // safety check + utils::path_safety_checker::RSSparams rss_params; + utils::path_safety_checker::RSSparams rss_params_for_abort; + // abort LaneChangeCancelParameters cancel; @@ -132,9 +137,9 @@ struct LaneChangeTargetObjectIndices struct LaneChangeTargetObjects { - std::vector current_lane{}; - std::vector target_lane{}; - std::vector other_lane{}; + std::vector current_lane{}; + std::vector target_lane{}; + std::vector other_lane{}; }; enum class LaneChangeModuleType { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 26163bcfd8058..f59e0170bdf14 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -19,7 +19,8 @@ #include "behavior_path_planner/parameters.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/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -43,10 +44,10 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -57,8 +58,16 @@ using tier4_autoware_utils::Polygon2d; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); +double calcMaximumLaneChangeLength( + const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const std::vector & shift_intervals, const double max_acc); + +double calcMinimumAcceleration( + const double current_velocity, const double min_longitudinal_acc, + const BehaviorPathPlannerParameters & params); + double calcMaximumAcceleration( - const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, + const double current_velocity, const double current_max_velocity, const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); double calcLaneChangingAcceleration( @@ -76,8 +85,8 @@ std::vector replaceWithSortedIds( const std::vector> & sorted_lane_ids); std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const double rough_shift_length); + const RouteHandler & route_handler, const Pose & current_pose, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); @@ -90,6 +99,10 @@ lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const LaneChangeModuleType & type); +lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type); + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); @@ -174,12 +187,6 @@ boost::optional getLeadingStaticObjectIdx( std::optional createPolygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); -LaneChangeTargetObjectIndices filterObject( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, - const Pose & current_pose, const RouteHandler & route_handler, - const LaneChangeParameters & lane_change_parameters); - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp new file mode 100644 index 0000000000000..b4575eb4d3b7e --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -0,0 +1,179 @@ +// 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_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; + +/** + * @brief Filters objects based on various criteria. + * + * @param objects The predicted objects to filter. + * @param route_handler + * @param current_lanes + * @param current_pose The current pose of ego vehicle. + * @param params The filtering parameters. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjects( + const std::shared_ptr & objects, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const geometry_msgs::msg::Point & current_pose, + const std::shared_ptr & params); + +/** + * @brief Filter objects based on their velocity. + * + * @param objects The predicted objects to filter. + * @param lim_v Velocity limit for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); + +/** + * @brief Filter objects based on a velocity range. + * + * @param objects The predicted objects to filter. + * @param min_v Minimum velocity for filtering. + * @param max_v Maximum velocity for filtering. + * @return PredictedObjects The filtered objects. + */ +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v); + +/** + * @brief Filter objects based on their position relative to a current_pose. + * + * @param objects The predicted objects to filter. + * @param path_points Points on the path. + * @param current_pose Current pose of the reference (e.g., ego vehicle). + * @param forward_distance Maximum forward distance for filtering. + * @param backward_distance Maximum backward distance for filtering. + */ +void filterObjectsByPosition( + PredictedObjects & objects, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance); +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param target_object_types The types of objects to retain after filtering. + */ +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Separate index of the obstacles into two part based on whether the object is within + * lanelet. + * @return Indices of objects pair. first objects are in the lanelet, and second others are out of + * lanelet. + */ +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Separate the objects into two part based on whether the object is within lanelet. + * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. + */ +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Get the predicted path from an object. + * + * @param obj The extended predicted object. + * @param is_use_all_predicted_path Flag to determine whether to use all predicted paths or just the + * one with maximum confidence. + * @return std::vector The predicted path(s) from the object. + */ +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); + +/** + * @brief Create a predicted path based on ego vehicle parameters. + * + * @param ego_predicted_path_params Parameters for ego's predicted path. + * @param path_points Points on the path. + * @param vehicle_pose Current pose of the ego-vehicle. + * @param current_velocity Current velocity of the vehicle. + * @param ego_seg_idx Index of the ego segment. + * @return std::vector The predicted path. + */ +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx); + +/** + * @brief Checks if the centroid of a given object is within the provided lanelets. + * + * @param object The predicted object to check. + * @param target_lanelets The lanelets to check against. + * @return bool True if the object's centroid is within the lanelets, otherwise false. + */ +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); + +/** + * @brief Transforms a given object into an extended predicted object. + * + * @param object The predicted object to transform. + * @param safety_check_time_horizon The time horizon for safety checks. + * @param safety_check_time_resolution The time resolution for safety checks. + * @return ExtendedPredictedObject The transformed object. + */ +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution); + +/** + * @brief Creates target objects on a lane based on provided parameters. + * + * @param current_lanes + * @param route_handler + * @param filtered_objects The filtered objects. + * @param params The filtering parameters. + * @return TargetObjectsOnLane The target objects on the lane. + */ +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, + const std::shared_ptr & params); + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp new file mode 100644 index 0000000000000..c4a6a86861e6c --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -0,0 +1,176 @@ +// 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_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ + +#include + +#include +#include + +#include + +namespace behavior_path_planner::utils::path_safety_checker +{ + +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::Polygon2d; + +struct PoseWithVelocity +{ + Pose pose; + double velocity{0.0}; + + PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} +}; + +struct PoseWithVelocityStamped : public PoseWithVelocity +{ + double time{0.0}; + + PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) + : PoseWithVelocity(pose, velocity), time(time) + { + } +}; + +struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped +{ + Polygon2d poly; + + PoseWithVelocityAndPolygonStamped( + const double time, const Pose & pose, const double velocity, const Polygon2d & poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + { + } +}; + +struct PredictedPathWithPolygon +{ + float confidence{0.0}; + std::vector path; +}; + +struct ExtendedPredictedObject +{ + unique_identifier_msgs::msg::UUID uuid; + geometry_msgs::msg::PoseWithCovariance initial_pose; + geometry_msgs::msg::TwistWithCovariance initial_twist; + geometry_msgs::msg::AccelWithCovariance initial_acceleration; + autoware_auto_perception_msgs::msg::Shape shape; + std::vector predicted_paths; +}; + +/** + * @brief Specifies which object class should be checked. + */ +struct ObjectTypesToCheck +{ + bool check_car{true}; ///< Check for cars. + bool check_truck{true}; ///< Check for trucks. + bool check_bus{true}; ///< Check for buses. + bool check_trailer{true}; ///< Check for trailers. + bool check_unknown{true}; ///< Check for unknown object types. + bool check_bicycle{true}; ///< Check for bicycles. + bool check_motorcycle{true}; ///< Check for motorcycles. + bool check_pedestrian{true}; ///< Check for pedestrians. +}; + +/** + * @brief Configuration for which lanes should be checked for objects. + */ +struct ObjectLaneConfiguration +{ + bool check_current_lane{}; ///< Check the current lane. + bool check_right_lane{}; ///< Check the right lane. + bool check_left_lane{}; ///< Check the left lane. + bool check_shoulder_lane{}; ///< Check the shoulder lane. + bool check_other_lane{}; ///< Check other lanes. +}; + +/** + * @brief Contains objects on lanes type. + */ +struct TargetObjectsOnLane +{ + std::vector on_current_lane{}; ///< Objects on the current lane. + std::vector on_right_lane{}; ///< Objects on the right lane. + std::vector on_left_lane{}; ///< Objects on the left lane. + std::vector on_shoulder_lane{}; ///< Objects on the shoulder lane. + std::vector on_other_lane{}; ///< Objects on other lanes. +}; + +/** + * @brief Parameters related to the RSS (Responsibility-Sensitive Safety) model. + */ +struct RSSparams +{ + double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle. + double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle. + double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance. + double longitudinal_distance_min_threshold{ + 0.0}; ///< Minimum threshold for longitudinal distance. + double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. + double front_vehicle_deceleration; ///< brake parameter + double rear_vehicle_deceleration; ///< brake parameter +}; + +/** + * @brief Parameters for generating the ego vehicle's predicted path. + */ +struct EgoPredictedPathParams +{ + double acceleration; ///< Acceleration value. + double time_horizon; ///< Time horizon for prediction. + double time_resolution; ///< Time resolution for prediction. + double min_slow_speed; ///< Minimum slow speed. + double delay_until_departure; ///< Delay before departure. + double target_velocity; ///< Target velocity. +}; + +/** + * @brief Parameters for filtering objects. + */ +struct ObjectsFilteringParams +{ + double safety_check_time_horizon; ///< Time horizon for object's prediction. + double safety_check_time_resolution; ///< Time resolution for object's prediction. + double object_check_forward_distance; ///< Forward distance for object checks. + double object_check_backward_distance; ///< Backward distance for object checks. + double ignore_object_velocity_threshold; ///< Velocity threshold for ignoring objects. + ObjectTypesToCheck object_types_to_check; ///< Specifies which object types to check. + ObjectLaneConfiguration object_lane_configuration; ///< Configuration for which lanes to check. + bool include_opposite_lane; ///< Include the opposite lane in checks. + bool invert_opposite_lane; ///< Invert the opposite lane in checks. + bool check_all_predicted_path; ///< Check all predicted paths. + bool use_all_predicted_path; ///< Use all predicted paths. + bool use_predicted_path_outside_lanelet; ///< Use predicted paths outside of lanelets. +}; + +/** + * @brief Parameters for safety checks. + */ +struct SafetyCheckParams +{ + bool enable_safety_check; ///< Enable safety checks. + double backward_lane_length; ///< Length of the backward lane for path generation. + double forward_path_length; ///< Length of the forward path lane for path generation. + RSSparams rss_params; ///< Parameters related to the RSS model. + bool publish_debug_marker{false}; ///< Option to publish debug markers. +}; + +} // namespace behavior_path_planner::utils::path_safety_checker + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp similarity index 69% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index d2671ff32a787..6d9df7677ead5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -36,7 +37,7 @@ #include #include -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { using autoware_auto_perception_msgs::msg::PredictedObject; @@ -49,51 +50,6 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; -struct PoseWithVelocity -{ - Pose pose; - double velocity{0.0}; - - PoseWithVelocity(const Pose & pose, const double velocity) : pose(pose), velocity(velocity) {} -}; - -struct PoseWithVelocityStamped : public PoseWithVelocity -{ - double time{0.0}; - - PoseWithVelocityStamped(const double time, const Pose & pose, const double velocity) - : PoseWithVelocity(pose, velocity), time(time) - { - } -}; - -struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped -{ - Polygon2d poly; - - PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) - { - } -}; - -struct PredictedPathWithPolygon -{ - float confidence{0.0}; - std::vector path; -}; - -struct ExtendedPredictedObject -{ - unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector predicted_paths; -}; - namespace bg = boost::geometry; bool isTargetObjectFront( @@ -109,8 +65,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, @@ -142,9 +97,18 @@ bool checkCollision( const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug); + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug); -} // namespace behavior_path_planner::utils::safety_check +/** + * @brief Check collision between ego path footprints with extra longitudinal stopping margin and + * objects. + * @return Has collision or not + */ +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double margin, const double max_stopping_margin); +} // namespace behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER__UTILS__SAFETY_CHECK_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index bdca6a2fdf085..191cd3e10def5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -75,7 +75,8 @@ std::pair getPathTurnSignal( const BehaviorPathPlannerParameters & common_parameter); PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets); std::vector getReversingIndices(const PathWithLaneId & path); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp new file mode 100644 index 0000000000000..abdf17c9c6cfe --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -0,0 +1,53 @@ +// 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_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ + +#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::RRTStar; + +class FreespacePullOut : public PullOutPlannerBase +{ +public: + FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info); + + PlannerType getPlannerType() override { return PlannerType::FREESPACE; } + + boost::optional plan(Pose start_pose, Pose end_pose) override; + +protected: + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp index d68985ec27350..608b918e839fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp @@ -19,6 +19,7 @@ #include +#include #include namespace behavior_path_planner @@ -27,6 +28,8 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index b95d247b6d23f..6e6e5111e5dd9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -39,6 +39,8 @@ enum class PlannerType { NONE = 0, SHIFT = 1, GEOMETRIC = 2, + STOP = 3, + FREESPACE = 4, }; class PullOutPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 7928938ac4d5b..2042593064c51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -45,10 +45,6 @@ class ShiftPullOut : public PullOutPlannerBase const BehaviorPathPlannerParameters & common_parameter, const behavior_path_planner::StartPlannerParameters & parameter); - bool hasEnoughDistance( - const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); - double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 8c21d065ef852..1e26bef0c85be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -18,11 +18,20 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include +#include +#include + #include #include namespace behavior_path_planner { + +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStarParam; + struct StartPlannerParameters { double th_arrived_distance; @@ -33,6 +42,7 @@ struct StartPlannerParameters double length_ratio_for_turn_signal_deactivation_near_intersection; double collision_check_margin; double collision_check_distance_from_end; + double th_moving_object_velocity; // shift pull out bool enable_shift_pull_out; bool check_shift_path_lane_departure; @@ -55,6 +65,17 @@ struct StartPlannerParameters double backward_search_resolution; double backward_path_update_duration; double ignore_distance_from_lane_end; + // freespace planner + bool enable_freespace_planner; + std::string freespace_planner_algorithm; + double end_pose_search_start_distance; + double end_pose_search_end_distance; + double end_pose_search_interval; + double freespace_planner_velocity; + double vehicle_shape_margin; + PlannerCommonParam freespace_planner_common_parameters; + AstarParam astar_parameters; + RRTStarParam rrt_star_parameters; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 6c7c6804ba6bb..63e374e074a5a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -28,6 +28,7 @@ #include #include +#include #include namespace behavior_path_planner::start_planner_utils @@ -47,6 +48,13 @@ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); +/** + * @brief calculate end arc length to generate reference path considering the goal position + * @return a pair of s_end and terminal_is_goal + */ +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose); } // namespace behavior_path_planner::start_planner_utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index ea217c8f7d0d4..be227f4f7e93e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -19,7 +19,8 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" -#include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "motion_utils/motion_utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" @@ -66,15 +67,19 @@ 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 behavior_path_planner::utils::safety_check::ExtendedPredictedObject; -using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; -using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; -using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; +using drivable_area_expansion::DrivableAreaExpansionParameters; 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; @@ -159,6 +164,16 @@ double getDistanceBetweenPredictedPathAndObject( const PredictedObject & object, const PredictedPath & path, const double start_time, const double end_time, const double resolution); +/** + * @brief Check collision between ego path footprints with extra longitudinal stopping margin and + * objects. + * @return Has collision or not + */ +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double margin, const double max_stopping_margin); + /** * @brief Check collision between ego path footprints and objects. * @return Has collision or not @@ -198,27 +213,6 @@ double calcLongitudinalDistanceFromEgoToObjects( const Pose & ego_pose, double base_link2front, double base_link2rear, const PredictedObjects & dynamic_objects); -/** - * @brief Separate index of the obstacles into two part based on whether the object is within - * lanelet. - * @return Indices of objects pair. first objects are in the lanelet, and second others are out of - * lanelet. - */ -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -/** - * @brief Separate the objects into two part based on whether the object is within lanelet. - * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. - */ -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); - -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); - // drivable area generation lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes); lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes); @@ -229,6 +223,9 @@ boost::optional getLeftLanelet( std::vector generateDrivableLanes(const lanelet::ConstLanelets & current_lanes); std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); +std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes, + const DrivableAreaExpansionParameters & parameters); std::vector calcBound( const std::shared_ptr route_handler, const std::vector & drivable_lanes, @@ -379,7 +376,7 @@ lanelet::ConstLanelets getExtendedCurrentLanes( lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, - const double forward_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, @@ -387,9 +384,6 @@ lanelet::ConstLanelets calcLaneAroundPose( const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); double calcMinimumLaneChangeLength( @@ -418,6 +412,9 @@ void makeBoundLongitudinallyMonotonic( std::optional getPolygonByPoint( const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, const std::string & polygon_name); + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ 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 29f1ee762abdf..3023357bd0a96 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -68,6 +68,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); + reroute_availability_publisher_ = + create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); debug_lane_change_msg_array_publisher_ = @@ -131,7 +133,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_ = std::make_shared(*this, p.verbose); const auto register_and_create_publisher = [&](const auto & manager) { - const auto & module_name = manager->getModuleName(); + const auto & module_name = manager->name(); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( module_name, create_publisher(path_candidate_name_space + module_name, 1)); @@ -337,8 +339,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); p.lane_changing_lateral_jerk = declare_parameter("lane_change.lane_changing_lateral_jerk"); - p.lateral_acc_switching_velocity = - declare_parameter("lane_change.lateral_acc_switching_velocity"); p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); p.minimum_lane_changing_velocity = declare_parameter("lane_change.minimum_lane_changing_velocity"); @@ -391,23 +391,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold"); - p.longitudinal_distance_min_threshold = - declare_parameter("longitudinal_distance_min_threshold"); - p.longitudinal_velocity_delta_time = - declare_parameter("longitudinal_velocity_delta_time"); - - p.expected_front_deceleration = declare_parameter("expected_front_deceleration"); - p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration"); - - p.expected_front_deceleration_for_abort = - declare_parameter("expected_front_deceleration_for_abort"); - p.expected_rear_deceleration_for_abort = - declare_parameter("expected_rear_deceleration_for_abort"); - - p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time"); - p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin"); - 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."); @@ -541,6 +524,9 @@ void BehaviorPathPlannerNode::run() // compute turn signal computeTurnSignal(planner_data_, *path, output); + // publish reroute availability + publish_reroute_availability(); + // publish drivable bounds publish_bounds(*path); @@ -656,6 +642,26 @@ void BehaviorPathPlannerNode::publish_steering_factor( steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } +void BehaviorPathPlannerNode::publish_reroute_availability() +{ + const bool has_approved_modules = planner_manager_->hasApprovedModules(); + const bool has_candidate_modules = planner_manager_->hasCandidateModules(); + + // In the current behavior path planner, we might get unexpected behavior when rerouting while + // modules other than lane follow are active. Therefore, rerouting will be allowed only when the + // lane follow module is running Note that if there is a approved module or candidate module, it + // means non-lane-following modules are runnning. + RerouteAvailability is_reroute_available; + is_reroute_available.stamp = this->now(); + if (has_approved_modules || has_candidate_modules) { + is_reroute_available.availability = false; + } else { + is_reroute_available.availability = true; + } + + reroute_availability_publisher_->publish(is_reroute_available); +} + void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data) { MarkerArray marker_array; @@ -779,27 +785,31 @@ void BehaviorPathPlannerNode::publishPathCandidate( const std::shared_ptr & planner_data) { for (auto & manager : managers) { - if (path_candidate_publishers_.count(manager->getModuleName()) == 0) { + if (path_candidate_publishers_.count(manager->name()) == 0) { continue; } - if (manager->getSceneModules().empty()) { - path_candidate_publishers_.at(manager->getModuleName()) + if (manager->getSceneModuleObservers().empty()) { + path_candidate_publishers_.at(manager->name()) ->publish(convertToPath(nullptr, false, planner_data)); continue; } - for (auto & module : manager->getSceneModules()) { - const auto & status = module->getCurrentStatus(); + for (auto & observer : manager->getSceneModuleObservers()) { + if (observer.expired()) { + continue; + } + const auto & status = observer.lock()->getCurrentStatus(); const auto candidate_path = std::invoke([&]() { if (status == ModuleStatus::SUCCESS || status == ModuleStatus::FAILURE) { // clear candidate path if the module is finished return convertToPath(nullptr, false, planner_data); } - return convertToPath(module->getPathCandidate(), module->isExecutionReady(), planner_data); + return convertToPath( + observer.lock()->getPathCandidate(), observer.lock()->isExecutionReady(), planner_data); }); - path_candidate_publishers_.at(module->name())->publish(candidate_path); + path_candidate_publishers_.at(observer.lock()->name())->publish(candidate_path); } } } @@ -809,19 +819,22 @@ void BehaviorPathPlannerNode::publishPathReference( const std::shared_ptr & planner_data) { for (auto & manager : managers) { - if (path_reference_publishers_.count(manager->getModuleName()) == 0) { + if (path_reference_publishers_.count(manager->name()) == 0) { continue; } - if (manager->getSceneModules().empty()) { - path_reference_publishers_.at(manager->getModuleName()) + if (manager->getSceneModuleObservers().empty()) { + path_reference_publishers_.at(manager->name()) ->publish(convertToPath(nullptr, false, planner_data)); continue; } - for (auto & module : manager->getSceneModules()) { - path_reference_publishers_.at(module->name()) - ->publish(convertToPath(module->getPathReference(), true, planner_data)); + for (auto & observer : manager->getSceneModuleObservers()) { + if (observer.expired()) { + continue; + } + path_reference_publishers_.at(observer.lock()->name()) + ->publish(convertToPath(observer.lock()->getPathReference(), true, planner_data)); } } } diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 061cb10883f6c..94073ad467326 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -222,62 +222,6 @@ MarkerArray createEgoStatusMarkerArray( return msg; } -MarkerArray createSafetyCheckMarkerArray( - const AvoidanceState & state, const Pose & pose, const DebugData & data) -{ - const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - MarkerArray msg; - - if (data.exist_adjacent_objects) { - auto marker = createDefaultMarker( - "map", current_time, "safety_alert", 0L, Marker::CYLINDER, createMarkerScale(0.2, 0.2, 2.0), - createMarkerColor(1.0, 1.0, 0.0, 0.8)); - - marker.color = state == AvoidanceState::YIELD ? createMarkerColor(1.0, 0.0, 0.0, 0.8) - : createMarkerColor(1.0, 1.0, 0.0, 0.8); - - marker.pose = calcOffsetPose(pose, 0.0, 1.5, 1.0); - msg.markers.push_back(marker); - - marker.pose = calcOffsetPose(pose, 0.0, -1.5, 1.0); - marker.id++; - msg.markers.push_back(marker); - } - - if (state == AvoidanceState::YIELD) { - return msg; - } - - { - auto marker = createDefaultMarker( - "map", current_time, "safety_longitudinal_margin", 0L, Marker::CUBE, - createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 0.1)); - - for (const auto & m : data.margin_data_array) { - if (m.enough_lateral_margin) { - continue; - } - - constexpr double max_x = 10.0; - - const auto offset = 0.5 * (m.base_link2front + m.base_link2rear) - m.base_link2rear; - const auto diff = m.longitudinal_distance - m.longitudinal_margin; - const auto scale_x = std::min(max_x, 2.0 * (m.base_link2front + m.base_link2rear + diff)); - - const auto ratio = std::clamp(diff / max_x, 0.0, 1.0); - - marker.pose = calcOffsetPose(m.pose, offset, 0.0, 0.0); - marker.pose.position.z += 1.0; - marker.scale = createMarkerScale(scale_x, 2.0 * m.vehicle_width, 2.0); - marker.color = createMarkerColor(1.0 - ratio, ratio, 0.0, 0.1); - marker.id++; - msg.markers.push_back(marker); - } - } - - return msg; -} - MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w) diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 80fea95b03caa..d8c87b6d1b291 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -314,11 +314,11 @@ MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3 MarkerArray createPolygonMarkerArray( const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, - const float & g, const float & b) + const float & g, const float & b, const float & w) { Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast(lane_id), Marker::LINE_STRIP, - createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); + createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a3309968dd62c..c09df4151ca35 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -49,15 +49,18 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); }); auto result_output = [&]() { - const bool is_any_approved_module_running = std::any_of( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING; }); + const bool is_any_approved_module_running = + std::any_of(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { + return m->getCurrentStatus() == ModuleStatus::RUNNING || + m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + }); // IDLE is a state in which an execution has been requested but not yet approved. // once approved, it basically turns to running. const bool is_any_candidate_module_running_or_idle = std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING || + m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL || m->getCurrentStatus() == ModuleStatus::IDLE; }); @@ -126,6 +129,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da return BehaviorModuleOutput{}; }(); + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); + generateCombinedDrivableArea(result_output, data); return result_output; @@ -209,14 +215,14 @@ std::vector PlannerManager::getRequestModules( }; for (const auto & manager_ptr : manager_ptrs_) { - stop_watch_.tic(manager_ptr->getModuleName()); + stop_watch_.tic(manager_ptr->name()); /** * don't launch candidate module if approved modules already exist. */ if (!approved_module_ptrs_.empty()) { if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -225,21 +231,20 @@ std::vector PlannerManager::getRequestModules( * launch new candidate module. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_same_name_module = [&name](const auto & m) { return m->name() == name; }; const auto itr = std::find_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_same_name_module); if (itr == candidate_module_ptrs_.end()) { if (manager_ptr->canLaunchNewModule()) { - const auto new_module_ptr = manager_ptr->getNewModule(); - - if (manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { - request_modules.emplace_back(new_module_ptr); + manager_ptr->updateIdleModuleInstance(); + if (manager_ptr->isExecutionRequested(previous_module_output)) { + request_modules.emplace_back(manager_ptr->getIdleModule()); } } - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -249,7 +254,7 @@ std::vector PlannerManager::getRequestModules( * candidate. if locked, break this loop. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_block_module = [&name](const auto & m) { return m->name() == name && m->isLockedNewModuleLaunch(); }; @@ -259,7 +264,7 @@ std::vector PlannerManager::getRequestModules( if (itr != candidate_module_ptrs_.end()) { request_modules.clear(); request_modules.emplace_back(*itr); - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); break; } } @@ -268,14 +273,14 @@ std::vector PlannerManager::getRequestModules( * module already exist. keep using it as candidate. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_launched_module = [&name](const auto & m) { return m->name() == name; }; const auto itr = std::find_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_launched_module); if (itr != candidate_module_ptrs_.end()) { request_modules.emplace_back(*itr); - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -285,20 +290,20 @@ std::vector PlannerManager::getRequestModules( */ { if (!manager_ptr->canLaunchNewModule()) { - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } - const auto new_module_ptr = manager_ptr->getNewModule(); - if (!manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { - toc(manager_ptr->getModuleName()); + manager_ptr->updateIdleModuleInstance(); + if (!manager_ptr->isExecutionRequested(previous_module_output)) { + toc(manager_ptr->name()); continue; } - request_modules.emplace_back(new_module_ptr); + request_modules.emplace_back(manager_ptr->getIdleModule()); } - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); } return request_modules; @@ -366,7 +371,8 @@ std::pair PlannerManager::runRequestModule const auto & manager_ptr = getManager(module_ptr); if (!manager_ptr->exist(module_ptr)) { - manager_ptr->registerNewModule(module_ptr, previous_module_output); + manager_ptr->registerNewModule( + std::weak_ptr(module_ptr), previous_module_output); } results.emplace(module_ptr->name(), run(module_ptr, data, previous_module_output)); @@ -393,6 +399,9 @@ std::pair PlannerManager::runRequestModule executable_modules.erase( std::remove_if(executable_modules.begin(), executable_modules.end(), remove_expired_modules), executable_modules.end()); + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } /** @@ -439,12 +448,20 @@ std::pair PlannerManager::runRequestModule BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) { - const bool has_any_running_candidate_module = hasAnyRunningCandidateModule(); - std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); results.emplace("root", output); + if (approved_module_ptrs_.empty()) { + return output; + } + + // lock approved modules besides last one + std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { + m->lockOutputPath(); + }); + approved_module_ptrs_.back()->unlockOutputPath(); + /** * execute all approved modules. */ @@ -464,19 +481,22 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisWaitingApproval(); }; const auto itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules); + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), waiting_approval_modules); - if (itr != approved_module_ptrs_.end()) { - clearCandidateModules(); - candidate_module_ptrs_.push_back(*itr); + if (itr != approved_module_ptrs_.rend()) { + const auto is_last_module = std::distance(approved_module_ptrs_.rbegin(), itr) == 0; + if (is_last_module) { + clearCandidateModules(); + candidate_module_ptrs_.push_back(*itr); - std::for_each( - std::next(itr), approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); }); + debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); - debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); - } + approved_module_ptrs_.pop_back(); + } - approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); + } } /** @@ -492,6 +512,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrupdateObserver(); }); + if (itr != approved_module_ptrs_.end()) { clearCandidateModules(); } @@ -514,59 +537,31 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() != ModuleStatus::SUCCESS; }); - - // convert reverse iterator -> iterator - const auto success_itr = std::prev(not_success_itr).base() - 1; - /** - * there is no succeeded module. return. + * remove success module immediately. if lane change module has succeeded, update root lanelet. */ - if (success_itr == approved_module_ptrs_.end()) { - return approved_modules_output; - } - - const auto lane_change_itr = std::find_if( - success_itr, approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - - /** - * remove success modules according to Last In First Out(LIFO) policy. when the next module is in - * ModuleStatus::RUNNING, the previous module keeps running even if it is in - * ModuleStatus::SUCCESS. - */ - if (lane_change_itr == approved_module_ptrs_.end() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { - debug_info_.emplace_back(m, Action::DELETE, "From Approved"); - deleteExpiredModules(m); - }); + { + const auto success_module_itr = std::partition( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); + const auto success_lane_change = std::any_of( + success_module_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - return approved_modules_output; - } + if (success_lane_change) { + root_lanelet_ = updateRootLanelet(data, true); + } - /** - * as an exception, when there is lane change module is in succeeded modules, it doesn't remove - * any modules if module whose status is NOT ModuleStatus::SUCCESS exists. this is because the - * root lanelet is updated at the moment of lane change module's unregistering, and that causes - * change First In module's input. - */ - if (not_success_itr == approved_module_ptrs_.rend() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { + std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); deleteExpiredModules(m); }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); + approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end()); - root_lanelet_ = updateRootLanelet(data); - - return approved_modules_output; + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } return approved_modules_output; @@ -601,6 +596,9 @@ void PlannerManager::updateCandidateModules( std::remove_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove), candidate_module_ptrs_.end()); + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } /** @@ -699,8 +697,8 @@ void PlannerManager::print() const string_stream << "-----------------------------------------------------------\n"; string_stream << "registered modules: "; for (const auto & m : manager_ptrs_) { - string_stream << "[" << m->getModuleName() << "]"; - max_string_num = std::max(max_string_num, m->getModuleName().length()); + string_stream << "[" << m->name() << "]"; + max_string_num = std::max(max_string_num, m->name().length()); } string_stream << "\n"; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 34977b0933019..ff5706c34bdc7 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -44,6 +45,7 @@ namespace behavior_path_planner { +using marker_utils::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -94,6 +96,11 @@ bool isDrivingSameLane( return !same_ids.empty(); } + +bool isBestEffort(const std::string & policy) +{ + return policy == "best_effort"; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -109,165 +116,125 @@ bool AvoidanceModule::isExecutionRequested() const { DEBUG_PRINT("AVOIDANCE isExecutionRequested"); - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - // Check ego is in preferred lane - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); // there is object that should be avoid. return true. - if (!!avoidance_data_.stop_target_object) { + if (!!avoid_data_.stop_target_object) { return true; } - if (avoidance_data_.unapproved_new_sl.empty()) { + if (avoid_data_.unapproved_new_sl.empty()) { return false; } - return !avoidance_data_.target_objects.empty(); + return !avoid_data_.target_objects.empty(); } bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoidance_data_.safe; + return avoid_data_.safe && avoid_data_.comfortable; } -ModuleStatus AvoidanceModule::updateState() +bool AvoidanceModule::canTransitSuccessState() { - const auto & data = avoidance_data_; - const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !data.target_objects.empty(); + const auto & data = avoid_data_; + // Change input lane. -> EXIT. if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); - return ModuleStatus::SUCCESS; - } - - const auto idx = planner_data_->findEgoIndex(data.reference_path.points); - if (idx == data.reference_path.points.size() - 1) { - arrived_path_end_ = true; - } - - constexpr double THRESHOLD = 1.0; - if ( - calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD && - arrived_path_end_) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "reach path end point. exit avoidance module."); - return ModuleStatus::SUCCESS; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Exit."); + return true; } - DEBUG_PRINT( - "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); + helper_.setPreviousDrivingLanes(data.current_lanelets); - if (!is_plan_running && !has_avoidance_target) { - return ModuleStatus::SUCCESS; - } + // Reach input path end point. -> EXIT. + { + const auto idx = planner_data_->findEgoIndex(data.reference_path.points); + if (idx == data.reference_path.points.size() - 1) { + arrived_path_end_ = true; + } - if ( - !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone && - !isAvoidanceManeuverRunning()) { - // if dynamic objects are removed on path, change current state to reset path - return ModuleStatus::SUCCESS; + constexpr double THRESHOLD = 1.0; + const auto is_further_than_threshold = + calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD; + if (is_further_than_threshold && arrived_path_end_) { + RCLCPP_WARN(getLogger(), "Reach path end point. Exit."); + return true; + } } - helper_.setPreviousDrivingLanes(data.current_lanelets); - - if (is_plan_running || current_state_ == ModuleStatus::RUNNING) { - return ModuleStatus::RUNNING; + const bool has_avoidance_target = !data.target_objects.empty(); + 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_avoidance_target) { + if (!has_shift_point && !has_base_offset) { + RCLCPP_INFO(getLogger(), "No objects. No approved shift lines. Exit."); + return true; + } } - return ModuleStatus::IDLE; -} - -bool AvoidanceModule::isAvoidancePlanRunning() const -{ - constexpr double AVOIDING_SHIFT_THR = 0.1; - const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > AVOIDING_SHIFT_THR; - const bool has_shift_point = (path_shifter_.getShiftLinesSize() > 0); - return has_base_offset || has_shift_point; -} -bool AvoidanceModule::isAvoidanceManeuverRunning() -{ - const auto path_idx = avoidance_data_.ego_closest_path_index; - for (const auto & al : registered_raw_shift_lines_) { - if (path_idx > al.start_idx || is_avoidance_maneuver_starts) { - is_avoidance_maneuver_starts = 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; + + return false; // Keep current state. } -AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const +void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) { - AvoidancePlanningData data; - // reference pose - const auto reference_pose = + data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_.getPreviousSplineShiftPath()); - data.reference_pose = reference_pose; - // special for avoidance: take behind distance upt ot shift-start-point if it exist. - const auto longest_dist_to_shift_line = [&]() { - double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); - } - for (const auto & sl : registered_raw_shift_lines_) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sl.start)); - } - return max_dist; - }(); - - // center line path (output of this function must have size > 1) - const auto center_path = utils::calcCenterLinePath( - planner_data_, reference_pose, longest_dist_to_shift_line, - *getPreviousModuleOutput().reference_path); - - debug.center_line = center_path; - if (center_path.points.size() < 2) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "calcCenterLinePath() must return path which size > 1"); - return data; - } + // lanelet info + data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( + *getPreviousModuleOutput().reference_path, planner_data_); // reference path - data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); + } else { + data.reference_path_rough = *getPreviousModuleOutput().path; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Don't use latest reference path."); + } + // resampled reference path data.reference_path = utils::resamplePathWithSpline( data.reference_path_rough, parameters_->resample_interval_for_planning); - if (data.reference_path.points.size() < 2) { - // if the resampled path has only 1 point, use original path. - data.reference_path = center_path; - } - - const size_t nearest_segment_index = - findNearestSegmentIndex(data.reference_path.points, data.reference_pose.position); - data.ego_closest_path_index = - std::min(nearest_segment_index + 1, data.reference_path.points.size() - 1); + // closest index + data.ego_closest_path_index = planner_data_->findEgoIndex(data.reference_path.points); // arclength from ego pose (used in many functions) data.arclength_from_ego = utils::calcPathArcLengthArray( data.reference_path, 0, data.reference_path.points.size(), calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); - // lanelet info - data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( - *getPreviousModuleOutput().reference_path, planner_data_); - - // keep avoidance state - data.state = avoidance_data_.state; - // target objects for avoidance fillAvoidanceTargetObjects(data, debug); - DEBUG_PRINT("target object size = %lu", data.target_objects.size()); + // lost object compensation + utils::avoidance::updateRegisteredObject(registered_objects_, data.target_objects, parameters_); + utils::avoidance::compensateDetectionLost( + registered_objects_, data.target_objects, data.other_objects); - return data; + // sort object order by longitudinal distance + std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) { + return a.longitudinal < b.longitudinal; + }); + + // set base path + path_shifter_.setPath(data.reference_path); } void AvoidanceModule::fillAvoidanceTargetObjects( @@ -278,12 +245,9 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::getTargetLanelets; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. - const auto expanded_lanelets = getTargetLanelets( - planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist, - parameters_->detection_area_right_expand_dist * (-1.0)); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + utils::avoidance::separateObjectsByPath( + helper_.getPreviousSplineShiftPath().path, planner_data_, data, parameters_, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -310,7 +274,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // debug { debug.current_lanelets = std::make_shared(data.current_lanelets); - debug.expanded_lanelets = std::make_shared(expanded_lanelets); std::vector debug_info_array; const auto append = [&](const auto & o) { @@ -465,7 +428,8 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Check avoidance path safety. For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.safe = isSafePath(path_shifter, data.candidate_path, debug); + data.comfortable = isComfortable(data.unapproved_new_sl); + data.safe = isSafePath(data.candidate_path, debug); } void AvoidanceModule::fillEgoStatus( @@ -491,18 +455,14 @@ void AvoidanceModule::fillEgoStatus( const auto can_yield_maneuver = canYieldManeuver(data); - const size_t ego_seg_idx = - planner_data_->findEgoSegmentIndex(helper_.getPreviousSplineShiftPath().path.points); - const auto offset = std::abs(motion_utils::calcLateralOffset( - helper_.getPreviousSplineShiftPath().path.points, getEgoPosition(), ego_seg_idx)); - - // don't output new candidate path if the offset between the ego and previous output path is - // larger than threshold. - // TODO(Satoshi OTA): remove this workaround - if (offset > parameters_->safety_check_ego_offset) { + /** + * If the output path is locked by outside of this module, don't update output path. + */ + if (isOutputPathLocked()) { data.safe_new_sl.clear(); data.candidate_path = helper_.getPreviousSplineShiftPath(); - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. canceling candidate path..."); + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 500, "this module is locked now. keep current path."); return; } @@ -641,20 +601,19 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif } case AvoidanceState::YIELD: { insertYieldVelocity(path); - insertWaitPoint(parameters_->use_constraints_for_decel, path); - removeRegisteredShiftLines(); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } case AvoidanceState::AVOID_PATH_NOT_READY: { - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } case AvoidanceState::AVOID_PATH_READY: { - insertWaitPoint(parameters_->use_constraints_for_decel, path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } case AvoidanceState::AVOID_EXECUTE: { - insertStopPoint(parameters_->use_constraints_for_decel, path); + insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); break; } default: @@ -666,7 +625,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif void AvoidanceModule::updateRegisteredRawShiftLines() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; utils::avoidance::fillAdditionalInfoFromPoint(data, registered_raw_shift_lines_); @@ -719,6 +678,8 @@ void AvoidanceModule::updateRegisteredRawShiftLines() AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( AvoidLineArray & raw_shift_lines, DebugData & debug) const { + const auto fill_gap_shift_lines = getFillGapShiftLines(raw_shift_lines); + /** * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. @@ -748,6 +709,8 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( * Add gap filled shift lines so that merged shift lines connect smoothly. */ fillShiftLineGap(raw_shift_lines); + raw_shift_lines.insert( + raw_shift_lines.end(), fill_gap_shift_lines.begin(), fill_gap_shift_lines.end()); debug.gap_filled = raw_shift_lines; /** @@ -785,7 +748,7 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) const auto old_size = registered_raw_shift_lines_.size(); auto future_with_info = future; - utils::avoidance::fillAdditionalInfoFromPoint(avoidance_data_, future_with_info); + utils::avoidance::fillAdditionalInfoFromPoint(avoid_data_, future_with_info); printShiftLines(future_with_info, "future_with_info"); printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_"); printShiftLines(current_raw_shift_lines_, "current_raw_shift_lines_"); @@ -852,6 +815,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto avoiding_shift = desire_shift_length - current_ego_shift; const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift); + if (!isBestEffort(parameters_->policy_lateral_margin)) { + return desire_shift_length; + } + // ego already has enough positive shift. const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3; if (is_object_on_right && has_enough_positive_shift) { @@ -864,6 +831,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( return desire_shift_length; } + // don't relax shift length since it can stop in front of the object. + if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) { + return desire_shift_length; + } + // calculate remaining distance. const auto prepare_distance = helper_.getNominalPrepareDistance(); const auto constant = @@ -900,7 +872,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } // avoidance distance is not enough. unavoidable. - if (!parameters_->use_constraints_for_decel) { + if (!isBestEffort(parameters_->policy_deceleration)) { object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; return boost::none; } @@ -971,23 +943,19 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( { const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front; const auto path_front_to_ego = - avoidance_data_.arclength_from_ego.at(avoidance_data_.ego_closest_path_index); + avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); al_avoid.start_longitudinal = std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( - avoidance_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); - al_avoid.start = avoidance_data_.reference_path.points.at(al_avoid.start_idx).point.pose; + avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); + al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); al_avoid.end_shift_length = feasible_shift_length.get(); al_avoid.end_longitudinal = o.longitudinal - offset; al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; - - if (is_valid_shift_line(al_avoid)) { - avoid_lines.push_back(al_avoid); - } } AvoidLine al_return; @@ -995,9 +963,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; // The end_margin also has the purpose of preventing the return path from NOT being // triggered at the end point. - const auto end_margin = 1.0; - const auto return_remaining_distance = - std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0); + const auto return_remaining_distance = std::max( + data.arclength_from_ego.back() - o.longitudinal - offset - + parameters_->remain_buffer_distance, + 0.0); al_return.start_shift_length = feasible_shift_length.get(); al_return.end_shift_length = 0.0; @@ -1006,10 +975,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; + } - if (is_valid_shift_line(al_return)) { - avoid_lines.push_back(al_return); - } + if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + avoid_lines.push_back(al_avoid); + avoid_lines.push_back(al_return); } o.is_avoidable = true; @@ -1045,8 +1015,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( void AvoidanceModule::generateTotalShiftLine( const AvoidLineArray & avoid_lines, ShiftLineData & shift_line_data) const { - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1095,7 +1065,10 @@ void AvoidanceModule::generateTotalShiftLine( // overwrite shift with current_ego_shift until ego pose. const auto current_shift = helper_.getEgoLinearShift(); - for (size_t i = 0; i <= avoidance_data_.ego_closest_path_index; ++i) { + for (size_t i = 0; i < sl.shift_line.size(); ++i) { + if (avoid_data_.ego_closest_path_index < i) { + break; + } sl.shift_line.at(i) = current_shift; sl.shift_line_grad.at(i) = 0.0; } @@ -1123,7 +1096,7 @@ void AvoidanceModule::generateTotalShiftLine( const auto grad_first_shift_line = (avoid_lines.front().start_shift_length - current_shift) / avoid_lines.front().start_longitudinal; - for (size_t i = avoidance_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { sl.shift_line.at(i) = helper_.getLinearShift(getPoint(path.points.at(i))); sl.shift_line_grad.at(i) = grad_first_shift_line; } @@ -1136,8 +1109,8 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ using utils::avoidance::setEndData; using utils::avoidance::setStartData; - const auto & path = avoidance_data_.reference_path; - const auto & arcs = avoidance_data_.arclength_from_ego; + const auto & path = avoid_data_.reference_path; + const auto & arcs = avoid_data_.arclength_from_ego; const auto N = path.points.size(); auto & sl = shift_line_data; @@ -1178,7 +1151,7 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ bool found_first_start = false; constexpr auto CREATE_SHIFT_GRAD_THR = 0.001; constexpr auto IS_ALREADY_SHIFTING_THR = 0.001; - for (size_t i = avoidance_data_.ego_closest_path_index; i < N - 1; ++i) { + for (size_t i = avoid_data_.ego_closest_path_index; i < N - 1; ++i) { const auto & p = path.points.at(i).point.pose; const auto shift = sl.shift_line.at(i); @@ -1222,6 +1195,38 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ return merged_avoid_lines; } +AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shift_lines) const +{ + AvoidLineArray ret{}; + + if (shift_lines.empty()) { + return ret; + } + + const auto calc_gap_shift_line = [&](const auto & line1, const auto & line2) { + AvoidLine gap_filled_line{}; + gap_filled_line.start_shift_length = line1.end_shift_length; + gap_filled_line.start_longitudinal = line1.end_longitudinal; + gap_filled_line.end_shift_length = line2.start_shift_length; + gap_filled_line.end_longitudinal = line2.start_longitudinal; + gap_filled_line.id = getOriginalShiftLineUniqueId(); + + return gap_filled_line; + }; + + // fill gap among shift lines. + for (size_t i = 0; i < shift_lines.size() - 1; i += 2) { + if (shift_lines.at(i).end_longitudinal > shift_lines.at(i + 1).start_longitudinal) { + continue; + } + ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1))); + } + + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + + return ret; +} + void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const { using utils::avoidance::setEndData; @@ -1230,7 +1235,7 @@ void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const return; } - const auto & data = avoidance_data_; + const auto & data = avoid_data_; helper_.alignShiftLinesOrder(shift_lines, false); @@ -1300,8 +1305,8 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( // debug print { - const auto & arc = avoidance_data_.arclength_from_ego; - const auto & closest = avoidance_data_.ego_closest_path_index; + const auto & arc = avoid_data_.arclength_from_ego; + const auto & closest = avoid_data_.ego_closest_path_index; const auto & sl = shift_line_data.shift_line; const auto & sg = shift_line_data.shift_line_grad; const auto & fg = shift_line_data.forward_grad; @@ -1640,7 +1645,7 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const { constexpr double ep = 1.0e-3; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; const bool has_candidate_point = !sl_candidates.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); @@ -1688,8 +1693,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_idx = avoid_data_.ego_closest_path_index; + last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = getCurrentBaseShift(); } } @@ -1730,12 +1735,12 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end_idx = avoidance_data_.ego_closest_path_index; - last_sl.end = avoidance_data_.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_idx = avoid_data_.ego_closest_path_index; + last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; last_sl.end_shift_length = current_base_shift; } - const auto & arclength_from_ego = avoidance_data_.arclength_from_ego; + const auto & arclength_from_ego = avoid_data_.arclength_from_ego; const auto nominal_prepare_distance = helper_.getNominalPrepareDistance(); const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); @@ -1744,14 +1749,14 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) return; } - const auto remaining_distance = arclength_from_ego.back(); + const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; // If the avoidance point has already been set, the return shift must be set after the point. - const auto last_sl_distance = avoidance_data_.arclength_from_ego.at(last_sl.end_idx); + const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); // check if there is enough distance for return. - if (last_sl_distance + 1.0 > remaining_distance) { // tmp: add some small number (+1.0) - DEBUG_PRINT("No enough distance for return."); + if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); return; } @@ -1800,7 +1805,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; @@ -1815,11 +1820,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.id = getOriginalShiftLineUniqueId(); al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.start = avoidance_data_.reference_path.points.at(al.start_idx).point.pose; + al.start = avoid_data_.reference_path.points.at(al.start_idx).point.pose; al.start_longitudinal = arclength_from_ego.at(al.start_idx); al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); - al.end = avoidance_data_.reference_path.points.at(al.end_idx).point.pose; + al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = 0.0; al.start_shift_length = last_sl.end_shift_length; @@ -1832,371 +1837,55 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) } bool AvoidanceModule::isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const + ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { - const auto & p = parameters_; + const auto & p = planner_data_->parameters; - if (!p->enable_safety_check) { + if (!parameters_->enable_safety_check) { return true; // if safety check is disabled, it always return safe. } - const auto & forward_check_distance = p->object_check_forward_distance; - const auto & backward_check_distance = p->safety_check_backward_distance; - const auto check_lanes = - getAdjacentLane(path_shifter, forward_check_distance, backward_check_distance); - - auto path_with_current_velocity = shifted_path.path; - - const size_t ego_idx = planner_data_->findEgoIndex(path_with_current_velocity.points); - utils::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0); - - constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38; // 5km/h - for (auto & p : path_with_current_velocity.points) { - p.point.longitudinal_velocity_mps = std::max(getEgoSpeed(), MIN_EGO_VEL_IN_PREDICTION); - } - - { - debug_data_.path_with_planned_velocity = path_with_current_velocity; - } - - return isSafePath(path_with_current_velocity, check_lanes, debug); -} + const auto ego_predicted_path = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); -bool AvoidanceModule::isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, DebugData & debug) const -{ - if (path.points.empty()) { - return true; - } - - const auto path_with_time = [&path]() { - std::vector> ret{}; - - float travel_time = 0.0; - ret.emplace_back(path.points.front(), travel_time); - - for (size_t i = 1; i < path.points.size(); ++i) { - const auto & p1 = path.points.at(i - 1); - const auto & p2 = path.points.at(i); - - const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); - const auto ds = calcDistance2d(p1, p2); - - travel_time += ds / v; - - ret.emplace_back(p2, travel_time); - } + 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++) { + const auto length = shifted_path.shift_length.at(i); - return ret; - }(); - - const auto move_objects = getAdjacentLaneObjects(check_lanes); - - { - debug.unsafe_objects.clear(); - debug.margin_data_array.clear(); - debug.exist_adjacent_objects = !move_objects.empty(); - } - - bool is_safe = true; - for (const auto & p : path_with_time) { - MarginData margin_data{}; - margin_data.pose = getPose(p.first); - - if (p.second > parameters_->safety_check_time_horizon) { - break; - } - - for (const auto & o : move_objects) { - const auto is_enough_margin = isEnoughMargin(p.first, p.second, o, margin_data); - - if (!is_enough_margin) { - debug.unsafe_objects.push_back(o); + if (parameters_->lateral_avoid_check_threshold < length) { + return false; } - is_safe = is_safe && is_enough_margin; - } - - debug.margin_data_array.push_back(margin_data); - } - - return is_safe; -} - -bool AvoidanceModule::isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const -{ - const auto & common_param = planner_data_->parameters; - const auto & vehicle_width = common_param.vehicle_width; - const auto & base_link2front = common_param.base_link2front; - const auto & base_link2rear = common_param.base_link2rear; - - const auto p_ref = [this, &p_ego]() { - const auto idx = findNearestIndex(avoidance_data_.reference_path.points, getPoint(p_ego)); - return getPose(avoidance_data_.reference_path.points.at(idx)); - }(); - - const auto & v_ego = p_ego.point.longitudinal_velocity_mps; - const auto & v_ego_lon = utils::avoidance::getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); - const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - - if (!utils::avoidance::isTargetObjectType(object.object, parameters_)) { - return true; - } - - // | centerline - // | ^ x - // | +-------+ | - // | | | | - // | | | D1 | D2 D4 - // | | obj |<-->|<---------->|<->| - // | | | D3 | +-------+ - // | | |<----------->| | - // | +-------+ | | | - // | | | ego | - // | | | | - // | | | | - // | | +-------+ - // | y <----+ - // D1: overhang_dist (signed value) - // D2: shift_length (signed value) - // D3: lateral_distance (should be larger than margin that's calculated from relative velocity.) - // D4: vehicle_width (unsigned value) - - const auto reliable_path = std::max_element( - object.object.kinematics.predicted_paths.begin(), - object.object.kinematics.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); - - if (reliable_path == object.object.kinematics.predicted_paths.end()) { - return true; - } - - const auto p_obj = [&t, &reliable_path]() { - boost::optional ret{boost::none}; - - const auto dt = rclcpp::Duration(reliable_path->time_step).seconds(); - const auto idx = static_cast(std::floor(t / dt)); - const auto res = t - dt * idx; - - if (idx > reliable_path->path.size() - 2) { - return ret; + if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { + return true; + } } - const auto & p_src = reliable_path->path.at(idx); - const auto & p_dst = reliable_path->path.at(idx + 1); - ret = calcInterpolatedPose(p_src, p_dst, res / dt); - return ret; + return std::nullopt; }(); - if (!p_obj) { - return true; - } - - const auto v_obj_lon = utils::avoidance::getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); - - double hysteresis_factor = 1.0; - if (avoidance_data_.state == AvoidanceState::YIELD) { - hysteresis_factor = parameters_->safety_check_hysteresis_factor; - } - - const auto shift_length = calcLateralDeviation(p_ref, getPoint(p_ego)); - const auto lateral_distance = std::abs(object.overhang_dist - shift_length) - 0.5 * vehicle_width; - const auto lateral_margin = getLateralMarginFromVelocity(std::abs(v_ego_lon - v_obj_lon)); - - if (lateral_distance > lateral_margin * hysteresis_factor) { - return true; - } - - const auto lon_deviation = calcLongitudinalDeviation(getPose(p_ego), p_obj.get().position); - const auto is_front_object = lon_deviation > 0.0; - const auto longitudinal_margin = - getRSSLongitudinalDistance(v_ego_lon, v_obj_lon, is_front_object); - const auto vehicle_offset = is_front_object ? base_link2front : base_link2rear; - const auto longitudinal_distance = - std::abs(lon_deviation) - vehicle_offset - 0.5 * object.object.shape.dimensions.x; - - { - margin_data.pose.orientation = p_ref.orientation; - margin_data.enough_lateral_margin = false; - margin_data.longitudinal_distance = - std::min(margin_data.longitudinal_distance, longitudinal_distance); - margin_data.longitudinal_margin = - std::max(margin_data.longitudinal_margin, longitudinal_margin); - margin_data.vehicle_width = vehicle_width; - margin_data.base_link2front = base_link2front; - margin_data.base_link2rear = base_link2rear; - } - - if (longitudinal_distance > longitudinal_margin * hysteresis_factor) { + if (!is_right_shift.has_value()) { return true; } - return false; -} - -double AvoidanceModule::getLateralMarginFromVelocity(const double velocity) const -{ - const auto & p = parameters_; - - if (p->col_size < 2 || p->col_size * 2 != p->target_velocity_matrix.size()) { - throw std::logic_error("invalid matrix col size."); - } - - if (velocity < p->target_velocity_matrix.front()) { - return p->target_velocity_matrix.at(p->col_size); - } - - if (velocity > p->target_velocity_matrix.at(p->col_size - 1)) { - return p->target_velocity_matrix.back(); - } - - for (size_t i = 1; i < p->col_size; ++i) { - if (velocity < p->target_velocity_matrix.at(i)) { - const auto v1 = p->target_velocity_matrix.at(i - 1); - const auto v2 = p->target_velocity_matrix.at(i); - const auto m1 = p->target_velocity_matrix.at(i - 1 + p->col_size); - const auto m2 = p->target_velocity_matrix.at(i + p->col_size); - - const auto v_clamp = std::clamp(velocity, v1, v2); - return m1 + (m2 - m1) * (v_clamp - v1) / (v2 - v1); - } - } - - return p->target_velocity_matrix.back(); -} - -double AvoidanceModule::getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const -{ - const auto & accel_for_rss = parameters_->safety_check_accel_for_rss; - const auto & idling_time = parameters_->safety_check_idling_time; + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( + avoid_data_, planner_data_, parameters_, is_right_shift.value()); - const auto opposite_lane_vehicle = v_obj < 0.0; - - /** - * object and ego already pass each other. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (!is_front_object && opposite_lane_vehicle) { - return 0.0; - } - - /** - * object drive opposite direction. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (is_front_object && opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) + - std::abs(v_obj) * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is in front of ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (is_front_object && !opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_obj, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is behind ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (!is_front_object && !opposite_lane_vehicle) { - return v_obj * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_ego, 2.0) / (2.0 * accel_for_rss); - } - - return 0.0; -} - -lanelet::ConstLanelets AvoidanceModule::getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const -{ - const auto & rh = planner_data_->route_handler; - - bool has_left_shift = false; - bool has_right_shift = false; - - for (const auto & sp : path_shifter.getShiftLines()) { - if (sp.end_shift_length > 0.01) { - has_left_shift = true; - continue; - } - - if (sp.end_shift_length < -0.01) { - has_right_shift = true; - continue; - } - } - - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Satoshi Ota) - } - - const auto ego_succeeding_lanes = - rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); - - lanelet::ConstLanelets check_lanes{}; - for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (has_left_shift && opt_left_lane) { - check_lanes.push_back(opt_left_lane.get()); - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (has_right_shift && opt_right_lane) { - check_lanes.push_back(opt_right_lane.get()); - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (has_right_shift && !right_opposite_lanes.empty()) { - check_lanes.push_back(right_opposite_lanes.front()); - } - } - - return check_lanes; -} - -ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( - const lanelet::ConstLanelets & adjacent_lanes) const -{ - ObjectDataArray objects; - for (const auto & o : avoidance_data_.other_objects) { - if (utils::avoidance::isCentroidWithinLanelets(o.object, adjacent_lanes)) { - objects.push_back(o); + for (const auto & object : safety_check_target_objects) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, parameters_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, + collision)) { + return false; + } } } - return objects; + return true; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const @@ -2209,7 +1898,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output }; const auto & route_handler = planner_data_->route_handler; - const auto & current_lanes = avoidance_data_.current_lanelets; + const auto & current_lanes = avoid_data_.current_lanelets; const auto & enable_opposite = parameters_->use_opposite_lane; std::vector drivable_lanes; @@ -2350,7 +2039,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output // generate obstacle polygons current_drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; @@ -2418,39 +2107,15 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig BehaviorModuleOutput AvoidanceModule::plan() { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; resetPathCandidate(); resetPathReference(); - /** - * Has new shift point? - * Yes -> Is it approved? - * Yes -> add the shift point. - * No -> set approval_handler to WAIT_APPROVAL state. - * No -> waiting approval? - * Yes -> clear WAIT_APPROVAL state. - * No -> do nothing. - */ - if (!data.safe_new_sl.empty()) { - debug_data_.new_shift_lines = data.safe_new_sl; - DEBUG_PRINT("new_shift_lines size = %lu", data.safe_new_sl.size()); - printShiftLines(data.safe_new_sl, "new_shift_lines"); + updatePathShifter(data.safe_new_sl); - const auto sl = helper_.getMainShiftLine(data.safe_new_sl); - if (helper_.getRelativeShiftToPath(sl) > 0.0) { - removePreviousRTCStatusRight(); - } else if (helper_.getRelativeShiftToPath(sl) < 0.0) { - removePreviousRTCStatusLeft(); - } else { - RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); - } - if (!parameters_->disable_path_update) { - addShiftLineIfApproved(data.safe_new_sl); - } - } else if (isWaitingApproval()) { - clearWaitingApproval(); - removeCandidateRTCStatus(); + if (data.yield_required) { + removeRegisteredShiftLines(); } // generate path with shift points that have been inserted. @@ -2494,16 +2159,22 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, parameters_->resample_interval_for_output); } - avoidance_data_.state = updateEgoState(data); + avoid_data_.state = updateEgoState(data); // update output data { updateEgoBehavior(data, spline_shift_path); - updateInfoMarker(avoidance_data_); - updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); + updateInfoMarker(avoid_data_); + updateDebugMarker(avoid_data_, path_shifter_, debug_data_); + } + + if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + output.path = std::make_shared(spline_shift_path.path); + } else { + output.path = getPreviousModuleOutput().path; + RCLCPP_WARN(getLogger(), "Previous module lane is updated. Do nothing."); } - output.path = std::make_shared(spline_shift_path.path); output.reference_path = getPreviousModuleOutput().reference_path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -2514,14 +2185,12 @@ BehaviorModuleOutput AvoidanceModule::plan() generateExtendedDrivableArea(output); setDrivableLanes(output.drivable_area_info.drivable_lanes); - updateRegisteredRTCStatus(spline_shift_path.path); - return output; } CandidateOutput AvoidanceModule::planCandidate() const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; CandidateOutput output; @@ -2563,71 +2232,53 @@ CandidateOutput AvoidanceModule::planCandidate() const BehaviorModuleOutput AvoidanceModule::planWaitingApproval() { - const auto & data = avoidance_data_; - - // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); if (path_shifter_.getShiftLines().empty()) { out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } - const auto all_unavoidable = std::all_of( - data.target_objects.begin(), data.target_objects.end(), - [](const auto & o) { return !o.is_avoidable; }); - - const auto candidate = planCandidate(); - if (!data.safe_new_sl.empty()) { - updateCandidateRTCStatus(candidate); - waitApproval(); - } else if (path_shifter_.getShiftLines().empty()) { - waitApproval(); - } else if (all_unavoidable) { - waitApproval(); - } else { - clearWaitingApproval(); - removeCandidateRTCStatus(); - } - - path_candidate_ = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; return out; } -void AvoidanceModule::addShiftLineIfApproved(const AvoidLineArray & shift_lines) +void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) { - if (isActivated()) { - DEBUG_PRINT("We want to add this shift point, and approved. ADD SHIFT POINT!"); - const size_t prev_size = path_shifter_.getShiftLinesSize(); - addNewShiftLines(path_shifter_, shift_lines); + if (parameters_->disable_path_update) { + return; + } - current_raw_shift_lines_ = avoidance_data_.unapproved_raw_sl; + if (shift_lines.empty()) { + return; + } - // register original points for consistency - registerRawShiftLines(shift_lines); + if (!isActivated()) { + return; + } - const auto sl = helper_.getMainShiftLine(shift_lines); - const auto sl_front = shift_lines.front(); - const auto sl_back = shift_lines.back(); + addNewShiftLines(path_shifter_, shift_lines); - if (helper_.getRelativeShiftToPath(sl) > 0.0) { - left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); - } else if (helper_.getRelativeShiftToPath(sl) < 0.0) { - right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); - } + current_raw_shift_lines_ = avoid_data_.unapproved_raw_sl; - uuid_map_.at("left") = generateUUID(); - uuid_map_.at("right") = generateUUID(); - candidate_uuid_ = generateUUID(); + registerRawShiftLines(shift_lines); - lockNewModuleLaunch(); + const auto sl = helper_.getMainShiftLine(shift_lines); + const auto sl_front = shift_lines.front(); + const auto sl_back = shift_lines.back(); - DEBUG_PRINT("shift_line size: %lu -> %lu", prev_size, path_shifter_.getShiftLinesSize()); - } else { - DEBUG_PRINT("We want to add this shift point, but NOT approved. waiting..."); - waitApproval(); + if (helper_.getRelativeShiftToPath(sl) > 0.0) { + left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); + } else if (helper_.getRelativeShiftToPath(sl) < 0.0) { + right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); } + + uuid_map_.at("left") = generateUUID(); + uuid_map_.at("right") = generateUUID(); + candidate_uuid_ = generateUUID(); + + lockNewModuleLaunch(); } /** @@ -2684,9 +2335,8 @@ void AvoidanceModule::addNewShiftLines( future.push_back(sl); } - const double road_velocity = - avoidance_data_.reference_path.points.at(front_new_shift_line.start_idx) - .point.longitudinal_velocity_mps; + const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) + .point.longitudinal_velocity_mps; const double shift_time = PathShifter::calcShiftTimeFromJerk( front_new_shift_line.getRelativeLength(), helper_.getLateralMaxJerkLimit(), helper_.getLateralMaxAccelLimit()); @@ -2713,12 +2363,16 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat std::abs(candidates.at(i).getRelativeLength()) > parameters_->lateral_small_shift_threshold) { if (has_large_shift) { - break; + return; } has_large_shift = true; } + if (!isComfortable(AvoidLineArray{candidates.at(i)})) { + return; + } + subsequent.push_back(candidates.at(i)); } }; @@ -2727,10 +2381,18 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat const auto get_subsequent_shift = [&, this](size_t i) { AvoidLineArray subsequent{candidates.at(i)}; + if (!isComfortable(subsequent)) { + return subsequent; + } + if (candidates.size() == i + 1) { return subsequent; } + if (!isComfortable(AvoidLineArray{candidates.at(i + 1)})) { + return subsequent; + } + if ( std::abs(candidates.at(i).getRelativeLength()) < parameters_->lateral_small_shift_threshold) { const auto has_large_shift = @@ -2747,13 +2409,6 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat return subsequent; }; - // check jerk limit. - const auto is_large_jerk = [this](const auto & s) { - const auto jerk = PathShifter::calcJerkFromLatLonDistance( - s.getRelativeLength(), s.getRelativeLongitudinal(), helper_.getAvoidanceEgoSpeed()); - return jerk > helper_.getLateralMaxJerkLimit(); - }; - // check ignore or not. const auto is_ignore_shift = [this](const auto & s) { return std::abs(helper_.getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold; @@ -2770,10 +2425,6 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat } if (!is_ignore_shift(candidate)) { - if (is_large_jerk(candidate)) { - break; - } - return get_subsequent_shift(i); } } @@ -2804,7 +2455,7 @@ bool AvoidanceModule::isValidShiftLine( constexpr double THRESHOLD = 0.1; const auto offset = std::abs(new_shift_length - helper_.getEgoShift()); if (offset > THRESHOLD) { - RCLCPP_WARN_THROTTLE( + RCLCPP_DEBUG_THROTTLE( getLogger(), *clock_, 1000, "new shift line is invalid. [HUGE OFFSET (%.2f)]", offset); return false; } @@ -2830,31 +2481,35 @@ void AvoidanceModule::updateData() } debug_data_ = DebugData(); - avoidance_data_ = calcAvoidancePlanningData(debug_data_); + avoid_data_ = AvoidancePlanningData(); - utils::avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, parameters_); - utils::avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + // update base path and target objects. + fillFundamentalData(avoid_data_, debug_data_); - std::sort( - avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), - [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); - - path_shifter_.setPath(avoidance_data_.reference_path); + // an empty path will kill further processing + if (avoid_data_.reference_path.points.empty()) { + return; + } // update registered shift point for new reference path & remove past objects updateRegisteredRawShiftLines(); - fillShiftLine(avoidance_data_, debug_data_); - fillEgoStatus(avoidance_data_, debug_data_); - fillDebugData(avoidance_data_, debug_data_); + // update shift line and check path safety. + fillShiftLine(avoid_data_, debug_data_); + + // update ego behavior. + fillEgoStatus(avoid_data_, debug_data_); + + // update debug data. + fillDebugData(avoid_data_, debug_data_); + + // update rtc status. + updateRTCData(); } void AvoidanceModule::processOnEntry() { initVariables(); - waitApproval(); } void AvoidanceModule::processOnExit() @@ -2881,8 +2536,6 @@ void AvoidanceModule::initVariables() void AvoidanceModule::initRTCStatus() { - removeRTCStatus(); - clearWaitingApproval(); left_shift_array_.clear(); right_shift_array_.clear(); uuid_map_.at("left") = generateUUID(); @@ -2890,6 +2543,41 @@ void AvoidanceModule::initRTCStatus() candidate_uuid_ = generateUUID(); } +void AvoidanceModule::updateRTCData() +{ + const auto & data = avoid_data_; + + updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); + + const auto candidates = data.safe ? data.safe_new_sl : data.unapproved_new_sl; + + if (candidates.empty()) { + removeCandidateRTCStatus(); + return; + } + + const auto shift_line = helper_.getMainShiftLine(candidates); + if (helper_.getRelativeShiftToPath(shift_line) > 0.0) { + removePreviousRTCStatusRight(); + } else if (helper_.getRelativeShiftToPath(shift_line) < 0.0) { + removePreviousRTCStatusLeft(); + } else { + RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); + } + + CandidateOutput output; + + 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); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; + + updateCandidateRTCStatus(output); +} + TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const { const auto shift_lines = path_shifter_.getShiftLines(); @@ -2995,6 +2683,7 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; + using marker_utils::createPolygonMarkerArray; using marker_utils::createPoseMarkerArray; using marker_utils::createShiftGradMarkerArray; using marker_utils::createShiftLengthMarkerArray; @@ -3004,7 +2693,6 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; - using marker_utils::avoidance_marker::createSafetyCheckMarkerArray; using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; using tier4_autoware_utils::appendMarkerArray; @@ -3040,10 +2728,8 @@ void AvoidanceModule::updateDebugMarker( helper_.getPreviousLinearShiftPath().path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); - add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); + add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); add(createOtherObjectsMarkerArray( data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); @@ -3181,7 +2867,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (!data.stop_target_object) { return; @@ -3224,7 +2910,7 @@ void AvoidanceModule::insertWaitPoint( void AvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.safe) { return; @@ -3272,7 +2958,7 @@ void AvoidanceModule::insertStopPoint( void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const { const auto & p = parameters_; - const auto & data = avoidance_data_; + const auto & data = avoid_data_; if (data.target_objects.empty()) { return; @@ -3289,7 +2975,7 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - const auto & data = avoidance_data_; + const auto & data = avoid_data_; // do nothing if there is no avoidance target. if (data.target_objects.empty()) { @@ -3330,6 +3016,15 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; // 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) { + utils::avoidance::insertDecelPoint( + getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, + slow_pose_); + return; + } + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 1ba7c588c84d6..c13c237673625 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -50,16 +50,10 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = get_parameter(node, ns + "resample_interval_for_output"); - p.detection_area_right_expand_dist = - get_parameter(node, ns + "detection_area_right_expand_dist"); - p.detection_area_left_expand_dist = - get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); + p.enable_cancel_maneuver = get_parameter(node, ns + "enable_cancel_maneuver"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); - p.enable_safety_check = get_parameter(node, ns + "enable_safety_check"); p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); @@ -137,23 +131,48 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); } - // safety check + // safety check general params { std::string ns = "avoidance.safety_check."; + p.enable_safety_check = get_parameter(node, ns + "enable"); + p.check_current_lane = get_parameter(node, ns + "check_current_lane"); + p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); + p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); + p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); + p.check_other_object = get_parameter(node, ns + "check_other_object"); + p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); + p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); + p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_time_horizon = get_parameter(node, ns + "safety_check_time_horizon"); - p.safety_check_idling_time = get_parameter(node, ns + "safety_check_idling_time"); - p.safety_check_accel_for_rss = get_parameter(node, ns + "safety_check_accel_for_rss"); p.safety_check_hysteresis_factor = get_parameter(node, ns + "safety_check_hysteresis_factor"); - p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); + } + + // safety check rss params + { + std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, ns + "lateral_distance_max_threshold"); } // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; - p.road_shoulder_safety_margin = get_parameter(node, ns + "road_shoulder_safety_margin"); + p.soft_road_shoulder_margin = get_parameter(node, ns + "soft_road_shoulder_margin"); + p.hard_road_shoulder_margin = get_parameter(node, ns + "hard_road_shoulder_margin"); p.lateral_execution_threshold = get_parameter(node, ns + "lateral_execution_threshold"); p.lateral_small_shift_threshold = get_parameter(node, ns + "lateral_small_shift_threshold"); @@ -168,6 +187,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = get_parameter(node, ns + "prepare_time"); p.min_prepare_distance = get_parameter(node, ns + "min_prepare_distance"); + p.remain_buffer_distance = get_parameter(node, ns + "remain_buffer_distance"); p.min_slow_down_speed = get_parameter(node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = get_parameter(node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = get_parameter(node, ns + "nominal_avoidance_speed"); @@ -186,10 +206,21 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.stop_buffer = get_parameter(node, ns + "stop_buffer"); } - // constraints + // policy { - std::string ns = "avoidance.constraints."; - p.use_constraints_for_decel = get_parameter(node, ns + "use_constraints_for_decel"); + std::string ns = "avoidance.policy."; + p.policy_deceleration = get_parameter(node, ns + "deceleration"); + p.policy_lateral_margin = get_parameter(node, ns + "lateral_margin"); + p.use_shorten_margin_immediately = + get_parameter(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) @@ -227,13 +258,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( } } - // velocity matrix - { - std::string ns = "avoidance.target_velocity_matrix."; - p.col_size = get_parameter(node, ns + "col_size"); - p.target_velocity_matrix = get_parameter>(node, ns + "matrix"); - } - // shift line pipeline { std::string ns = "avoidance.shift_line_pipeline."; @@ -307,8 +331,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorlateral_small_shift_threshold); updateParam( parameters, ns + "lateral_avoid_check_threshold", p->lateral_avoid_check_threshold); - updateParam( - parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin); + updateParam(parameters, ns + "soft_road_shoulder_margin", p->soft_road_shoulder_margin); + updateParam(parameters, ns + "hard_road_shoulder_margin", p->hard_road_shoulder_margin); } { @@ -367,8 +391,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorsharp_shift_filter_threshold); } - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 444515f6cd398..8e6ab24929a00 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -41,13 +41,13 @@ geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & return geom_obj_point; } -std::pair getMinMaxValues(const std::vector & vec) +MinMaxValue getMinMaxValues(const std::vector & vec) { const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end())); const size_t max_idx = std::distance(vec.begin(), std::max_element(vec.begin(), vec.end())); - return std::make_pair(vec.at(min_idx), vec.at(max_idx)); + return MinMaxValue{vec.at(min_idx), vec.at(max_idx)}; } void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose) @@ -56,7 +56,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid", marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE, tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0), - tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8)); + tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); marker.pose = obj_pose; marker_array.markers.push_back(marker); @@ -69,7 +69,7 @@ void appendExtractedPolygonMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), visualization_msgs::msg::Marker::LINE_STRIP, tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8)); + tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8)); // NOTE: obj_poly.outer() has already duplicated points to close the polygon. for (size_t i = 0; i < obj_poly.outer().size(); ++i) { @@ -101,15 +101,20 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & path_points, const PredictedObject & object) { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double obj_vel_yaw = std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); return std::make_pair( - obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw)); + obj_vel_norm * std::cos(obj_vel_yaw - path_yaw), + obj_vel_norm * std::sin(obj_vel_yaw - path_yaw)); } double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) @@ -181,13 +186,31 @@ bool isLeft( } return false; } + +template +std::optional getObstacleFromUuid( + const std::vector & obstacles, const std::string & target_uuid) +{ + const auto itr = std::find_if(obstacles.begin(), obstacles.end(), [&](const auto & obstacle) { + return obstacle.uuid == target_uuid; + }); + + if (itr == obstacles.end()) { + return std::nullopt; + } + return *itr; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{std::move(parameters)}, + target_objects_manager_{TargetObjectsManager( + parameters_->successive_num_to_entry_dynamic_avoidance_condition, + parameters_->successive_num_to_exit_dynamic_avoidance_condition)} { } @@ -223,16 +246,14 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { - // calculate target objects candidate - const auto target_objects_candidate = calcTargetObjectsCandidate(); + // calculate target objects + updateTargetObjects(); - // calculate target objects considering flickering suppress + const auto target_objects_candidate = target_objects_manager_.getValidObjects(); target_objects_.clear(); for (const auto & target_object_candidate : target_objects_candidate) { - if ( - parameters_->successive_num_to_entry_dynamic_avoidance_condition <= - target_object_candidate.alive_counter) { - target_objects_.push_back(target_object_candidate.object); + if (target_object_candidate.should_be_avoided) { + target_objects_.push_back(target_object_candidate); } } } @@ -254,29 +275,40 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() debug_marker_.markers.clear(); const auto prev_module_path = getPreviousModuleOutput().path; - const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; - prev_objects_min_bound_lat_offset_.resetCurrentUuids(); for (const auto & object : target_objects_) { - const auto obstacle_poly = calcDynamicObstaclePolygon(object); + const auto obstacle_poly = [&]() { + if (parameters_->polygon_generation_method == "ego_path_base") { + return calcEgoPathBasedDynamicObstaclePolygon(object); + } + if (parameters_->polygon_generation_method == "object_path_base") { + return calcObjectPathBasedDynamicObstaclePolygon(object); + } + throw std::logic_error("The polygon_generation_method's string is invalid."); + }(); if (obstacle_poly) { - obstacles_for_drivable_area.push_back({object.pose, obstacle_poly.value(), object.is_left}); + obstacles_for_drivable_area.push_back( + {object.pose, obstacle_poly.value(), object.is_collision_left}); appendObjectMarker(info_marker_, object.pose); appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); - - prev_objects_min_bound_lat_offset_.addCurrentUuid(object.uuid); } } - prev_objects_min_bound_lat_offset_.removeCounterUnlessUpdated(); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = + getPreviousModuleOutput().drivable_area_info.drivable_lanes; + current_drivable_area_info.obstacles = obstacles_for_drivable_area; + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; BehaviorModuleOutput output; output.path = prev_module_path; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; - output.drivable_area_info.drivable_lanes = drivable_lanes; - output.drivable_area_info.obstacles = obstacles_for_drivable_area; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; return output; @@ -325,97 +357,177 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const return false; } -std::vector -DynamicAvoidanceModule::calcTargetObjectsCandidate() +void DynamicAvoidanceModule::updateTargetObjects() { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - // convert predicted objects to dynamic avoidance objects - std::vector output_objects_candidate; + const auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + const auto prev_objects = target_objects_manager_.getValidObjects(); + + // 1. Rough filtering of target objects + target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; - const auto obj_path = *std::max_element( - predicted_object.kinematics.predicted_paths.begin(), - predicted_object.kinematics.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + const double obj_vel_norm = std::hypot( + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); + const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); - // 1. check label + // 1.a. check label const bool is_label_target_obstacle = isLabelTargetObstacle(predicted_object.classification.front().label); if (!is_label_target_obstacle) { continue; } - // 2. check if velocity is large enough + // 1.b. check if velocity is large enough const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { continue; } - // 3. check if object is not crossing ego's path + // 1.c. check if object is not crossing ego's path const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); - const bool is_obstacle_crossing_path = - parameters_->max_crossing_object_angle < std::abs(obj_angle) && - parameters_->max_crossing_object_angle < M_PI - std::abs(obj_angle); + const double max_crossing_object_angle = 0.0 <= obj_tangent_vel + ? parameters_->max_overtaking_crossing_object_angle + : parameters_->max_oncoming_crossing_object_angle; + const bool is_obstacle_crossing_path = max_crossing_object_angle < std::abs(obj_angle) && + max_crossing_object_angle < M_PI - std::abs(obj_angle); + const double min_crossing_object_vel = 0.0 <= obj_tangent_vel + ? parameters_->min_overtaking_crossing_object_vel + : parameters_->min_oncoming_crossing_object_vel; const bool is_crossing_object_to_ignore = - parameters_->min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; + min_crossing_object_vel < obj_vel_norm && is_obstacle_crossing_path; if (is_crossing_object_to_ignore) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it crosses the ego's path.", + obj_uuid.c_str()); continue; } - // 4. check if object is not to be followed by ego + // 1.e. check if object lateral offset to ego's path is small enough const double obj_dist_to_path = calcDistanceToPath(prev_module_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( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since lateral offset is large.", obj_uuid.c_str()); + continue; + } + + // 1.f. calculate the object is on ego's path or not const bool is_object_on_ego_path = obj_dist_to_path < planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path; - if (is_object_on_ego_path && std::abs(obj_angle) < parameters_->max_front_object_angle) { - continue; - } - // 5. check if object lateral offset to ego's path is large enough - const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); - if (is_object_far_from_path) { + // 1.g. calculate latest time inside ego's path + const auto latest_time_inside_ego_path = [&]() -> std::optional { + if (!prev_object || !prev_object->latest_time_inside_ego_path) { + if (is_object_on_ego_path) { + return clock_->now(); + } + return std::nullopt; + } + if (is_object_on_ego_path) { + return clock_->now(); + } + return *prev_object->latest_time_inside_ego_path; + }(); + + const auto target_object = DynamicAvoidanceObject( + predicted_object, obj_tangent_vel, obj_normal_vel, is_object_on_ego_path, + latest_time_inside_ego_path); + target_objects_manager_.updateObject(obj_uuid, target_object); + } + target_objects_manager_.finalize(); + + // 2. Precise filtering of target objects and check if they should be avoided + for (const auto & object : target_objects_manager_.getValidObjects()) { + const auto obj_uuid = object.uuid; + const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); + const auto obj_path = *std::max_element( + object.predicted_paths.begin(), object.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // 2.a. check if object is not to be followed by ego + const double obj_angle = calcDiffAngleAgainstPath(prev_module_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); + if (object.is_object_on_ego_path && is_object_aligned_to_path) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it is to be followed.", obj_uuid.c_str()); continue; } - // 6. calculate which side object exists against ego's path - const bool is_left = isLeft(prev_module_path->points, obj_pose.position); + // 2.b. calculate which side object exists against ego's path + const bool is_object_left = isLeft(prev_module_path->points, object.pose.position); + const auto lat_lon_offset = + getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape); - // 6. check if object will not cut in or cut out + // 2.c. check if object will not cut in const bool will_object_cut_in = - willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel); - const bool will_object_cut_out = willObjectCutOut(obj_tangent_vel, obj_normal_vel, is_left); - if (will_object_cut_in || will_object_cut_out) { + willObjectCutIn(prev_module_path->points, obj_path, object.vel, lat_lon_offset); + if (will_object_cut_in) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since it will cut in.", obj_uuid.c_str()); + continue; + } + + // 2.d. check if object will not cut out + const auto will_object_cut_out = + willObjectCutOut(object.vel, object.lat_vel, is_object_left, prev_object); + if (will_object_cut_out.decision) { + printIgnoreReason(obj_uuid.c_str(), will_object_cut_out.reason); continue; } - // 7. check if time to collision + // 2.e. check time to collision const double time_to_collision = - calcTimeToCollision(prev_module_path->points, obj_pose, obj_tangent_vel); - - // 8. calculate alive counter for filtering objects - const auto prev_target_object_candidate = - DynamicAvoidanceObjectCandidate::getObjectFromUuid(prev_target_objects_candidate_, obj_uuid); - const int alive_counter = - prev_target_object_candidate - ? std::min( - parameters_->successive_num_to_entry_dynamic_avoidance_condition, - prev_target_object_candidate->alive_counter + 1) - : 0; + calcTimeToCollision(prev_module_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 target_object = DynamicAvoidanceObject( - predicted_object, obj_tangent_vel, obj_normal_vel, is_left, time_to_collision); - const auto target_object_candidate = - DynamicAvoidanceObjectCandidate{target_object, alive_counter}; - output_objects_candidate.push_back(target_object_candidate); + // 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(prev_module_path->points, future_obj_pose->position) + : is_object_left; + + // 2.g. calculate longitudinal and lateral offset to avoid + const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( + path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); + const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( + path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); + + 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); } - - prev_target_objects_candidate_ = output_objects_candidate; - return output_objects_candidate; } [[maybe_unused]] std::optional> @@ -458,15 +570,24 @@ DynamicAvoidanceModule::calcCollisionSection( double DynamicAvoidanceModule::calcTimeToCollision( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const double obj_tangent_vel) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { - const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + // 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 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 signed_lon_length / positive_relative_velocity; + return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity); } bool DynamicAvoidanceModule::isObjectFarFromPath( @@ -481,7 +602,7 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { constexpr double epsilon_path_lat_diff = 0.3; @@ -490,6 +611,20 @@ bool DynamicAvoidanceModule::willObjectCutIn( return false; } + // Ignore object close to the ego + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + 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.min_lon_offset; + if ( + lon_offset_ego_to_obj < std::max( + parameters_->min_lon_offset_ego_to_cut_in_object, + relative_velocity * parameters_->min_time_to_start_cut_in)) { + return false; + } + for (const auto & predicted_path_point : predicted_path.path) { const double paths_lat_diff = motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); @@ -500,25 +635,45 @@ bool DynamicAvoidanceModule::willObjectCutIn( return false; } -bool DynamicAvoidanceModule::willObjectCutOut( - const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const +DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( + const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, + const std::optional & prev_object) const { // Ignore oncoming object if (obj_tangent_vel < 0) { - return false; + return DecisionWithReason{false}; } - constexpr double object_lat_vel_thresh = 0.3; - if (is_left) { - if (object_lat_vel_thresh < obj_normal_vel) { - return true; + // Check if previous object is memorized + if (!prev_object || !prev_object->latest_time_inside_ego_path) { + return DecisionWithReason{false}; + } + if ( + parameters_->max_time_from_outside_ego_path_for_cut_out < + (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds()) { + return DecisionWithReason{false}; + } + + // Check object's lateral velocity + std::stringstream reason; + reason << "since latest time inside ego's path is small enough (" + << (clock_->now() - *prev_object->latest_time_inside_ego_path).seconds() << "<" + << parameters_->max_time_from_outside_ego_path_for_cut_out << ")"; + if (is_object_left) { + if (parameters_->min_cut_out_object_lat_vel < obj_normal_vel) { + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << "min_cut_out_object_lat_vel) { + reason << ", and lateral velocity is large enough (" + << parameters_->min_cut_out_object_lat_vel << " DynamicAvoidanceModule::getAdjacentLanes( @@ -561,105 +716,162 @@ std::pair DynamicAvoidanceModule return std::make_pair(right_lanes, left_lanes); } -// NOTE: object does not have const only to update min_bound_lat_offset. -std::optional DynamicAvoidanceModule::calcDynamicObstaclePolygon( - const DynamicAvoidanceObject & object) const +DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset( + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape) const { - const auto ego_pose = getEgoPose(); - const auto & rh = planner_data_->route_handler; + const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); + const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); - // get path with backward margin - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(ego_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"), - "failed to find closest lanelet within route!!!"); - return std::nullopt; + // TODO(murooka) calculation is not so accurate. + std::vector obj_lat_offset_vec; + std::vector obj_lon_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const size_t obj_point_seg_idx = + motion_utils::findNearestSegmentIndex(ego_path, geom_obj_point); + + // calculate lateral offset + const double obj_point_lat_offset = + motion_utils::calcLateralOffset(ego_path, geom_obj_point, obj_point_seg_idx); + obj_lat_offset_vec.push_back(obj_point_lat_offset); + + // calculate longitudinal offset + const double lon_offset = + motion_utils::calcLongitudinalOffsetToSegment(ego_path, obj_seg_idx, geom_obj_point); + obj_lon_offset_vec.push_back(lon_offset); } - auto path_with_backward_margin = [&]() { - constexpr double forward_length = 100.0; - const double backward_length = 50.0; - const auto current_lanes = - rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length); - return utils::getCenterLinePath( - *rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters); - }(); + const auto obj_lat_min_max_offset = getMinMaxValues(obj_lat_offset_vec); + const auto obj_lon_min_max_offset = getMinMaxValues(obj_lon_offset_vec); + + return LatLonOffset{ + obj_seg_idx, obj_lat_min_max_offset.max_value, obj_lat_min_max_offset.min_value, + obj_lon_min_max_offset.max_value, obj_lon_min_max_offset.min_value}; +} +MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const double time_to_collision) const +{ const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, object.pose.position); - const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, obj_pose.position); + // calculate min/max longitudinal offset from object to path + const auto obj_lon_offset = [&]() { + std::vector obj_lon_offset_vec; + for (size_t i = 0; i < obj_points.outer().size(); ++i) { + const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); + const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( + path_points_for_object_polygon, 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); + + if (obj_vel < 0) { + return MinMaxValue{ + raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value}; + } + + // 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 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); + + return MinMaxValue{ + obj_lon_offset.min_value - start_length_to_avoid, + obj_lon_offset.max_value + end_length_to_avoid}; +} + +MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( + const std::vector & path_points_for_object_polygon, + const Polygon2d & obj_points, const bool is_collision_left, const double obj_normal_vel, + const std::optional & prev_object) const +{ // calculate min/max lateral offset from object to path - const auto [min_obj_lat_abs_offset, max_obj_lat_abs_offset] = [&]() { + const auto obj_lat_abs_offset = [&]() { std::vector obj_lat_abs_offset_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(path_with_backward_margin.points, geom_obj_point); + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, geom_obj_point); const double obj_point_lat_offset = motion_utils::calcLateralOffset( - path_with_backward_margin.points, geom_obj_point, obj_point_seg_idx); + path_points_for_object_polygon, geom_obj_point, obj_point_seg_idx); obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset)); } return getMinMaxValues(obj_lat_abs_offset_vec); }(); - const double min_obj_lat_offset = min_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0); - const double max_obj_lat_offset = max_obj_lat_abs_offset * (object.is_left ? 1.0 : -1.0); + const double min_obj_lat_abs_offset = obj_lat_abs_offset.min_value; + const double max_obj_lat_abs_offset = obj_lat_abs_offset.max_value; - // calculate min/max longitudinal offset from object to path - const auto obj_lon_offset = [&]() -> std::optional> { - std::vector obj_lon_offset_vec; - for (size_t i = 0; i < obj_points.outer().size(); ++i) { - const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); - const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( - path_with_backward_margin.points, obj_seg_idx, geom_obj_point); - obj_lon_offset_vec.push_back(lon_offset); - } + // calculate bound min and max lateral offset + const double min_bound_lat_offset = [&]() { + const double lat_abs_offset_to_shift = + std::max(0.0, obj_normal_vel * (is_collision_left ? -1.0 : 1.0)) * + parameters_->max_time_for_lat_shift; + const double raw_min_bound_lat_offset = + min_obj_lat_abs_offset - parameters_->lat_offset_from_obstacle - lat_abs_offset_to_shift; + const double min_bound_lat_abs_offset_limit = + planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; + return std::max(raw_min_bound_lat_offset, min_bound_lat_abs_offset_limit) * + (is_collision_left ? 1.0 : -1.0); + }(); + const double max_bound_lat_offset = + (max_obj_lat_abs_offset + parameters_->lat_offset_from_obstacle) * + (is_collision_left ? 1.0 : -1.0); - const auto [raw_min_obj_lon_offset, raw_max_obj_lon_offset] = - getMinMaxValues(obj_lon_offset_vec); - if (object.time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + // filter min_bound_lat_offset + const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional { + if (!prev_object || !prev_object->lat_offset_to_avoid) { return std::nullopt; } - - if (0 <= object.vel) { - const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_overtaking_object, object.time_to_collision); - return std::make_pair( - raw_min_obj_lon_offset + object.vel * limited_time_to_collision, - raw_max_obj_lon_offset + object.vel * limited_time_to_collision); - } - - const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_oncoming_object, object.time_to_collision); - return std::make_pair( - raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset); + return prev_object->lat_offset_to_avoid->min_value; }(); + const double filtered_min_bound_lat_offset = + prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter( + min_bound_lat_offset, *prev_min_lat_avoid_to_offset, + parameters_->lpf_gain_for_lat_avoid_to_offset) + : min_bound_lat_offset; - if (!obj_lon_offset) { + return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset}; +} + +// NOTE: object does not have const only to update min_bound_lat_offset. +std::optional +DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const +{ + if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { return std::nullopt; } - const double min_obj_lon_offset = obj_lon_offset->first; - const double max_obj_lon_offset = obj_lon_offset->second; - // calculate bound start and end index - const bool is_object_overtaking = (0.0 <= object.vel); - const double start_length_to_avoid = - std::abs(object.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(object.vel) * (is_object_overtaking - ? parameters_->end_duration_to_avoid_overtaking_object - : parameters_->end_duration_to_avoid_oncoming_object); + auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + + const size_t obj_seg_idx = + motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, 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, min_obj_lon_offset - start_length_to_avoid, path_with_backward_margin.points); + obj_seg_idx, object.lon_offset_to_avoid->min_value, path_points_for_object_polygon); 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, max_obj_lon_offset + end_length_to_avoid, - path_with_backward_margin.points); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, path_points_for_object_polygon); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -669,58 +881,78 @@ std::optional DynamicAvoidanceModule::calcDynam 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(path_with_backward_margin.points.size() - 1); - - // calculate bound min and max lateral offset - const double min_bound_lat_offset = [&]() { - const double raw_min_bound_lat_offset = - min_obj_lat_offset - parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0); - const double min_bound_lat_abs_offset_limit = - planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; - if (object.is_left) { - if (raw_min_bound_lat_offset < min_bound_lat_abs_offset_limit) { - return min_bound_lat_abs_offset_limit; - } - } else { - if (-min_bound_lat_abs_offset_limit < raw_min_bound_lat_offset) { - return -min_bound_lat_abs_offset_limit; - } - } - return raw_min_bound_lat_offset; - }(); - const double max_bound_lat_offset = - max_obj_lat_offset + parameters_->lat_offset_from_obstacle * (object.is_left ? 1.0 : -1.0); - - // filter min_bound_lat_offset - const auto prev_min_bound_lat_offset = prev_objects_min_bound_lat_offset_.get(object.uuid); - const double filtered_min_bound_lat_offset = - prev_min_bound_lat_offset - ? signal_processing::lowpassFilter(min_bound_lat_offset, *prev_min_bound_lat_offset, 0.3) - : min_bound_lat_offset; - prev_objects_min_bound_lat_offset_.update(object.uuid, filtered_min_bound_lat_offset); + : static_cast(path_points_for_object_polygon.size() - 1); // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back( + obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->min_value, 0.0) + .position); + obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( + path_points_for_object_polygon.at(i).point.pose, 0.0, + object.lat_offset_to_avoid->max_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); + } + + boost::geometry::correct(obj_poly); + return obj_poly; +} + +std::optional +DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( + const DynamicAvoidanceObject & object) const +{ + const auto obj_path = *std::max_element( + object.predicted_paths.begin(), object.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // calculate left and right bound + std::vector obj_left_bound_points; + std::vector obj_right_bound_points; + for (size_t i = 0; i < obj_path.path.size(); ++i) { + const double lon_offset = [&]() { + if (i == 0) return -object.shape.dimensions.x / 2.0 - parameters_->lat_offset_from_obstacle; + if (i == obj_path.path.size() - 1) + return object.shape.dimensions.x / 2.0 + parameters_->lat_offset_from_obstacle; + return 0.0; + }(); + + const auto & obj_pose = obj_path.path.at(i); + obj_left_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( - path_with_backward_margin.points.at(i).point.pose, 0.0, filtered_min_bound_lat_offset, 0.0) + obj_pose, lon_offset, + object.shape.dimensions.y / 2.0 + parameters_->lat_offset_from_obstacle, 0.0) .position); - obj_outer_bound_points.push_back( + obj_right_bound_points.push_back( tier4_autoware_utils::calcOffsetPose( - path_with_backward_margin.points.at(i).point.pose, 0.0, max_bound_lat_offset, 0.0) + obj_pose, lon_offset, + -object.shape.dimensions.y / 2.0 - parameters_->lat_offset_from_obstacle, 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) { + for (const auto & bound_point : obj_right_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) { + std::reverse(obj_left_bound_points.begin(), obj_left_bound_points.end()); + for (const auto & bound_point : obj_left_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); } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index eddb93094574d..3b2e2caf92e6f 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -29,6 +29,12 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { DynamicAvoidanceParameters p{}; + { // common + std::string ns = "dynamic_avoidance.common."; + p.enable_debug_info = node->declare_parameter(ns + "enable_debug_info"); + p.use_hatched_road_markings = node->declare_parameter(ns + "use_hatched_road_markings"); + } + { // target object std::string ns = "dynamic_avoidance.target_object."; p.avoid_car = node->declare_parameter(ns + "car"); @@ -42,25 +48,47 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); + p.successive_num_to_exit_dynamic_avoidance_condition = + node->declare_parameter(ns + "successive_num_to_exit_dynamic_avoidance_condition"); p.min_obj_lat_offset_to_ego_path = node->declare_parameter(ns + "min_obj_lat_offset_to_ego_path"); p.max_obj_lat_offset_to_ego_path = node->declare_parameter(ns + "max_obj_lat_offset_to_ego_path"); + p.min_time_to_start_cut_in = + node->declare_parameter(ns + "cut_in_object.min_time_to_start_cut_in"); + p.min_lon_offset_ego_to_cut_in_object = + node->declare_parameter(ns + "cut_in_object.min_lon_offset_ego_to_object"); + + p.max_time_from_outside_ego_path_for_cut_out = + node->declare_parameter(ns + "cut_out_object.max_time_from_outside_ego_path"); + p.min_cut_out_object_lat_vel = + node->declare_parameter(ns + "cut_out_object.min_object_lat_vel"); + p.max_front_object_angle = node->declare_parameter(ns + "front_object.max_object_angle"); - p.min_crossing_object_vel = - node->declare_parameter(ns + "crossing_object.min_object_vel"); - p.max_crossing_object_angle = - node->declare_parameter(ns + "crossing_object.max_object_angle"); + p.min_overtaking_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_overtaking_object_vel"); + p.max_overtaking_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_overtaking_object_angle"); + p.min_oncoming_crossing_object_vel = + 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"); } { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.polygon_generation_method = + node->declare_parameter(ns + "polygon_generation_method"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); p.max_lat_offset_to_avoid = node->declare_parameter(ns + "max_lat_offset_to_avoid"); + p.max_time_for_lat_shift = + node->declare_parameter(ns + "max_time_for_object_lat_shift"); + p.lpf_gain_for_lat_avoid_to_offset = + node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); @@ -88,6 +116,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams( using tier4_autoware_utils::updateParam; auto & p = parameters_; + { // common + const std::string ns = "dynamic_avoidance.common."; + updateParam(parameters, ns + "enable_debug_info", p->enable_debug_info); + updateParam(parameters, ns + "use_hatched_road_markings", p->use_hatched_road_markings); + } + { // target object const std::string ns = "dynamic_avoidance.target_object."; @@ -105,26 +139,55 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", p->successive_num_to_entry_dynamic_avoidance_condition); + updateParam( + parameters, ns + "successive_num_to_exit_dynamic_avoidance_condition", + p->successive_num_to_exit_dynamic_avoidance_condition); updateParam( parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path); updateParam( parameters, ns + "max_obj_lat_offset_to_ego_path", p->max_obj_lat_offset_to_ego_path); + updateParam( + parameters, ns + "cut_in_object.min_time_to_start_cut_in", p->min_time_to_start_cut_in); + updateParam( + parameters, ns + "cut_in_object.min_lon_offset_ego_to_object", + p->min_lon_offset_ego_to_cut_in_object); + + updateParam( + parameters, ns + "cut_out_object.max_time_from_outside_ego_path", + p->max_time_from_outside_ego_path_for_cut_out); + updateParam( + parameters, ns + "cut_out_object.min_object_lat_vel", p->min_cut_out_object_lat_vel); + updateParam( parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); updateParam( - parameters, ns + "crossing_object.min_object_vel", p->min_crossing_object_vel); + parameters, ns + "crossing_object.min_overtaking_object_vel", + p->min_overtaking_crossing_object_vel); updateParam( - parameters, ns + "crossing_object.max_object_angle", p->max_crossing_object_angle); + parameters, ns + "crossing_object.max_overtaking_object_angle", + p->max_overtaking_crossing_object_angle); + updateParam( + parameters, ns + "crossing_object.min_oncoming_object_vel", + p->min_oncoming_crossing_object_vel); + updateParam( + parameters, ns + "crossing_object.max_oncoming_object_angle", + p->max_oncoming_crossing_object_angle); } { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; + updateParam( + parameters, ns + "polygon_generation_method", p->polygon_generation_method); updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); + updateParam( + parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift); + updateParam( + parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); updateParam( parameters, ns + "overtaking_object.max_time_to_collision", @@ -150,8 +213,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams( p->end_duration_to_avoid_oncoming_object); } - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f0aec7ba62d2f..f37149f706436 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -153,7 +153,8 @@ void GoalPlannerModule::onTimer() // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; double min_start_arc_length = std::numeric_limits::max(); @@ -309,7 +310,9 @@ bool GoalPlannerModule::isExecutionRequested() const const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = - allow_goal_modification_ ? calcModuleRequestLength() : parameters_->minimum_request_length; + goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) + ? calcModuleRequestLength() + : parameters_->minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -318,7 +321,7 @@ 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 (!allow_goal_modification_) { + if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { return goal_is_in_current_lanes; } @@ -404,7 +407,10 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const ModuleStatus GoalPlannerModule::updateState() { // finish module only when the goal is fixed - if (!allow_goal_modification_ && hasFinishedGoalPlanner()) { + if ( + !goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_) && + hasFinishedGoalPlanner()) { return ModuleStatus::SUCCESS; } @@ -418,9 +424,16 @@ bool GoalPlannerModule::planFreespacePath() mutex_.lock(); goal_searcher_->update(goal_candidates_); const auto goal_candidates = goal_candidates_; + debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); + debug_data_.freespace_planner.is_planning = true; mutex_.unlock(); - for (const auto & goal_candidate : goal_candidates) { + for (size_t i = 0; i < goal_candidates.size(); i++) { + const auto goal_candidate = goal_candidates.at(i); + mutex_.lock(); + debug_data_.freespace_planner.current_goal_idx = i; + mutex_.unlock(); + if (!goal_candidate.is_safe) { continue; } @@ -436,9 +449,12 @@ bool GoalPlannerModule::planFreespacePath() status_.is_safe = true; modified_goal_pose_ = goal_candidate; last_path_update_time_ = std::make_unique(clock_->now()); + debug_data_.freespace_planner.is_planning = false; mutex_.unlock(); return true; } + + debug_data_.freespace_planner.is_planning = false; return false; } @@ -491,7 +507,7 @@ void GoalPlannerModule::generateGoalCandidates() // calculate goal candidates const Pose goal_pose = route_handler->getGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { goal_searcher_->setPlannerData(planner_data_); goal_candidates_ = goal_searcher_->search(refined_goal_pose_); } else { @@ -506,13 +522,10 @@ BehaviorModuleOutput GoalPlannerModule::plan() { generateGoalCandidates(); - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { return planWithGoalModification(); } else { - // for fixed goals, only minor path refinements are made, - // so other modules are always allowed to run. - setIsSimultaneousExecutableAsApprovedModule(true); - setIsSimultaneousExecutableAsCandidateModule(true); fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } @@ -540,7 +553,9 @@ void GoalPlannerModule::selectSafePullOverPath() } // check if path is valid and safe - if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) { + if ( + !hasEnoughDistance(pull_over_path) || + checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) { continue; } @@ -590,7 +605,8 @@ void GoalPlannerModule::setLanes() { status_.current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); status_.pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); status_.lanes = @@ -602,7 +618,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) if (status_.is_safe) { // clear stop pose when the path is safe and activated if (isActivated()) { - status_.stop_pose.reset(); + resetWallPoses(); } // keep stop if not enough time passed, @@ -664,7 +680,8 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -816,18 +833,17 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() printParkingPositionError(); } + setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + return output; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { return planWaitingApprovalWithGoalModification(); } else { - // for fixed goals, only minor path refinements are made, - // so other modules are always allowed to run. - setIsSimultaneousExecutableAsApprovedModule(true); - setIsSimultaneousExecutableAsCandidateModule(true); fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } @@ -854,7 +870,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *out.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -869,6 +886,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); + setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + return out; } @@ -907,6 +926,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() 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; + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; if (status_.current_lanes.empty()) { @@ -944,13 +964,16 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // 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 auto min_stop_distance = calcFeasibleDecelDistance(0.0); - if (min_stop_distance && ego_to_stop_distance + stop_distance_buffer_ < *min_stop_distance) { + 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(); } // slow down for turn signal, insert stop point to stop_pose decelerateForTurnSignal(stop_pose, reference_path); - status_.stop_pose = stop_pose; + stop_pose_ = stop_pose; // slow down before the search area. if (search_start_offset_pose) { @@ -994,7 +1017,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { - status_.stop_pose = stop_path.points.at(*stop_idx).point.pose; + stop_pose_ = stop_path.points.at(*stop_idx).point.pose; } return stop_path; @@ -1064,11 +1087,22 @@ bool GoalPlannerModule::isStopped() bool GoalPlannerModule::isStuck() { + constexpr double stuck_time = 5.0; + if (!isStopped(odometry_buffer_stuck_, stuck_time)) { + return false; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + // any path has never been found if (!status_.pull_over_path) { return false; } - constexpr double stuck_time = 5.0; - return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); + + return checkCollision(getCurrentPath()); } bool GoalPlannerModule::hasFinishedCurrentPath() @@ -1081,17 +1115,20 @@ bool GoalPlannerModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -bool GoalPlannerModule::isOnGoal() const +bool GoalPlannerModule::isOnModifiedGoal() const { + if (!modified_goal_pose_) { + return false; + } + const Pose current_pose = planner_data_->self_odometry->pose.pose; - const Pose goal_pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose - : planner_data_->route_handler->getGoalPose(); - return calcDistance2d(current_pose, goal_pose) < parameters_->th_arrived_distance; + return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < + parameters_->th_arrived_distance; } bool GoalPlannerModule::hasFinishedGoalPlanner() { - return isOnGoal() && isStopped(); + return isOnModifiedGoal() && isStopped(); } TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const @@ -1108,8 +1145,15 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const const double distance_to_end = calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); const bool is_before_end_pose = distance_to_end >= 0.0; - turn_signal.turn_signal.command = - is_before_end_pose ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; + if (is_before_end_pose) { + if (left_side_parking_) { + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + } } // calc desired/required start/end point @@ -1136,9 +1180,14 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } if (parameters_->use_object_recognition) { - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path, *(planner_data_->dynamic_object), - parameters_->object_recognition_collision_check_margin)) { + const auto common_parameters = planner_data_->parameters; + const double base_link2front = common_parameters.base_link2front; + const double base_link2rear = common_parameters.base_link2rear; + const double vehicle_width = common_parameters.vehicle_width; + if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( + path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, + parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, + parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { return true; } } @@ -1157,13 +1206,12 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. const bool is_separated_path = status_.pull_over_path->partial_paths.size() > 1; - constexpr double eps_vel = 0.01; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; - if ( - is_separated_path && std::abs(current_vel) < eps_vel && - distance_to_start < distance_to_restart) { + const double eps_vel = 0.01; + const bool is_stopped = std::abs(current_vel) < eps_vel; + if (is_separated_path && is_stopped && distance_to_start < distance_to_restart) { return false; } @@ -1172,7 +1220,11 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - if (distance_to_start + stop_distance_buffer_ < *current_to_stop_distance) { + // If the stop line is subtly exceeded, it is assumed that there is not enough distance to the + // starting point of parking, so to prevent this, once the vehicle has stopped, it also has a + // stop_distance_buffer to allow for the amount exceeded. + const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; + if (distance_to_start + buffer < *current_to_stop_distance) { return false; } @@ -1400,11 +1452,15 @@ void GoalPlannerModule::setDebugData() const auto header = planner_data_->route_handler->getRouteHeader(); - const auto add = [this](const MarkerArray & added) { + const auto add = [this](MarkerArray added) { + for (auto & marker : added.markers) { + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - if (allow_goal_modification_) { + if (goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { // Visualize pull over areas const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1442,7 +1498,8 @@ void GoalPlannerModule::setDebugData() auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = modified_goal_pose_->goal_pose; + marker.pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose + : planner_data_->self_odometry->pose.pose; marker.text = magic_enum::enum_name(status_.pull_over_path->type); marker.text += " " + std::to_string(status_.current_path_idx) + "/" + std::to_string(status_.pull_over_path->partial_paths.size() - 1); @@ -1452,6 +1509,12 @@ void GoalPlannerModule::setDebugData() marker.text += " stopped"; } + if (debug_data_.freespace_planner.is_planning) { + marker.text += + " freespace: " + std::to_string(debug_data_.freespace_planner.current_goal_idx) + "/" + + std::to_string(debug_data_.freespace_planner.num_goal_candidates); + } + planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } @@ -1462,13 +1525,6 @@ void GoalPlannerModule::setDebugData() add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); } - - // Visualize stop pose - if (status_.stop_pose) { - add(createStopVirtualWallMarker( - *status_.stop_pose, "pull_over", clock_->now(), 0, - planner_data_->parameters.base_link2front)); - } } void GoalPlannerModule::printParkingPositionError() const @@ -1505,7 +1561,7 @@ bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const { - return !isOnGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); + return !isOnModifiedGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); } bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index caa75927f4062..7df1749a0fe82 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" + +#include #include #include @@ -85,6 +88,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); + p.object_recognition_collision_check_max_extra_stopping_margin = + node->declare_parameter( + ns + "object_recognition_collision_check_max_extra_stopping_margin"); } // pull over general params @@ -235,6 +241,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } parameters_ = std::make_shared(p); + + left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; } void GoalPlannerModuleManager::updateModuleParams( @@ -246,9 +254,33 @@ void GoalPlannerModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = name_ + "."; - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } +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, left_side_parking_)) { + return true; + } + + return enable_simultaneous_execution_as_approved_module_; +} + +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, left_side_parking_)) { + return true; + } + + return enable_simultaneous_execution_as_candidate_module_; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 0db16c4c1d4ca..841fc9092e279 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include @@ -136,7 +137,8 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( const auto p = std::dynamic_pointer_cast(avoidance_parameters_); const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, data.current_lanelets); + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, data.current_lanelets); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 00b280af43677..270e2064228af 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -15,7 +15,6 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" #include "behavior_path_planner/marker_utils/lane_change/debug.hpp" -#include "behavior_path_planner/module_status.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" @@ -36,7 +35,7 @@ using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map, + const std::unordered_map> & rtc_interface_ptr_map, std::unique_ptr && module_type) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)}, @@ -220,6 +219,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -230,7 +230,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { *prev_approved_path_ = *getPreviousModuleOutput().path; - module_type_->insertStopPoint(*prev_approved_path_); + module_type_->insertStopPoint( + module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; out.path = std::make_shared(*prev_approved_path_); @@ -278,10 +279,9 @@ CandidateOutput LaneChangeInterface::planCandidate() const return output; } -void LaneChangeInterface::updateModuleParams( - const std::shared_ptr & parameters) +void LaneChangeInterface::updateModuleParams(const std::any & parameters) { - parameters_ = parameters; + parameters_ = std::any_cast>(parameters); } void LaneChangeInterface::setData(const std::shared_ptr & data) @@ -336,7 +336,8 @@ std::shared_ptr LaneChangeInterface::get_debug_msg_arra debug_msg.is_front = debug_data.is_front; debug_msg.relative_distance = debug_data.relative_to_ego; debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = debug_data.object_twist.linear.x; + debug_msg.velocity = + std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y); debug_msg_array.lane_change_info.push_back(debug_msg); } lane_change_debug_msg_array_ = debug_msg_array; @@ -507,7 +508,7 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map > & rtc_interface_ptr_map) + const std::unordered_map> & rtc_interface_ptr_map) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, std::make_unique(parameters, avoidance_by_lane_change_parameters)} diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 18c669a207095..f392ef6a26e22 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -85,6 +85,36 @@ LaneChangeModuleManager::LaneChangeModuleManager( get_parameter(node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + + p.rss_params_for_abort.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params_for_abort.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params_for_abort.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; @@ -133,14 +163,14 @@ LaneChangeModuleManager::LaneChangeModuleManager( parameters_ = std::make_shared(p); } -std::shared_ptr LaneChangeModuleManager::createNewSceneModuleInstance() +std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() { if (type_ == LaneChangeModuleType::NORMAL) { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, std::make_unique(parameters_, direction_)); } @@ -155,8 +185,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } @@ -193,12 +223,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( get_parameter(node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = get_parameter(node, ns + "resample_interval_for_output"); - p.detection_area_right_expand_dist = - get_parameter(node, ns + "detection_area_right_expand_dist"); - p.detection_area_left_expand_dist = - get_parameter(node, ns + "detection_area_left_expand_dist"); - p.enable_update_path_when_object_is_gone = - get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); } @@ -271,10 +295,10 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( avoidance_parameters_ = std::make_shared(p); } -std::shared_ptr +std::unique_ptr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 7ce71a342de2b..3db3e049c3c81 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -125,6 +126,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() if (isAbortState()) { output.reference_path = std::make_shared(prev_module_reference_path_); + output.turn_signal_info = prev_turn_signal_info_; return output; } @@ -164,17 +166,38 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) utils::combineDrivableAreaInfo(current_drivable_area_info, prev_drivable_area_info_); } -void NormalLaneChange::insertStopPoint(PathWithLaneId & path) +void NormalLaneChange::insertStopPoint( + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { - const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane( - status_.lane_change_path.info.current_lanes.back()); + if (lanelets.empty()) { + return; + } + + const auto & route_handler = getRouteHandler(); + + if (route_handler->getNumLaneToPreferredLane(lanelets.back()) == 0) { + return; + } + + const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); const double lane_change_buffer = utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); - constexpr double stop_point_buffer{1.0}; - const auto stopping_distance = std::max( - motion_utils::calcArcLength(path.points) - lane_change_buffer - stop_point_buffer, 0.0); - const auto stop_point = utils::insertStopPoint(stopping_distance, path); + // If lanelets.back() is in goal route section, get distance to goal. + // Otherwise, get distance to end of lane. + double distance_to_terminal = 0.0; + if (route_handler->isInGoalRouteSection(lanelets.back())) { + const auto goal = route_handler->getGoalPose(); + distance_to_terminal = utils::getSignedDistance(path.points.front().point.pose, goal, lanelets); + } else { + 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 stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + if (stopping_distance > 0.0) { + const auto stop_point = utils::insertStopPoint(stopping_distance, path); + } } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -348,11 +371,16 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); - const auto & lane_change_path = status_.lane_change_path.path; const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; - const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( - lane_change_path.points, current_pose.position, lane_change_end.position); - const double finish_judge_buffer = planner_data_->parameters.lane_change_finish_judge_buffer; + 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; + + // If ego velocity is low, relax finish judge buffer + const double ego_velocity = getEgoVelocity(); + if (std::abs(ego_velocity) < 1.0) { + finish_judge_buffer = 0.0; + } const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0; if (!reach_lane_change_end) { @@ -471,6 +499,67 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } +std::vector NormalLaneChange::sampleLongitudinalAccValues( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (prev_module_path_.points.empty()) { + return {}; + } + + const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = *getRouteHandler(); + const auto current_pose = getEgoPose(); + const auto current_velocity = getEgoVelocity(); + + const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; + const auto vehicle_min_acc = + std::max(common_parameters.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto vehicle_max_acc = + std::min(common_parameters.max_acc, lane_change_parameters_->max_longitudinal_acc); + const double nearest_dist_threshold = common_parameters.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = common_parameters.ego_nearest_yaw_threshold; + + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_path_.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double & max_path_velocity = + prev_module_path_.points.at(current_seg_idx).point.longitudinal_velocity_mps; + + // calculate minimum and maximum acceleration + const auto min_acc = utils::lane_change::calcMinimumAcceleration( + current_velocity, vehicle_min_acc, common_parameters); + const auto max_acc = utils::lane_change::calcMaximumAcceleration( + current_velocity, max_path_velocity, vehicle_max_acc, common_parameters); + + // if max acc is not positive, then we do the normal sampling + if (max_acc <= 0.0) { + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + + // calculate maximum lane change length + const double max_lane_change_length = utils::lane_change::calcMaximumLaneChangeLength( + current_velocity, common_parameters, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + + if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + + // if maximum lane change length is less than length to goal or the end of target lanes, only + // sample max acc + if (route_handler.isInGoalRouteSection(target_lanes.back())) { + const auto goal_pose = route_handler.getGoalPose(); + if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + return {max_acc}; + } + } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + return {max_acc}; + } + + return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); +} + double NormalLaneChange::calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { @@ -516,9 +605,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = utils::lane_change::filterObject( - objects, current_lanes, target_lanes, target_backward_lanes, current_pose, route_handler, - *lane_change_parameters_); + const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -549,6 +636,114 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( return target_objects; } +LaneChangeTargetObjectIndices NormalLaneChange::filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto current_vel = getEgoVelocity(); + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + const auto & objects = *planner_data_->dynamic_object; + + // Guard + if (objects.objects.empty()) { + return {}; + } + + // Get path + const auto path = + route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_path = + route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + + const auto current_polygon = + utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto target_backward_polygon = utils::lane_change::createPolygon( + target_backward_lanes, 0.0, std::numeric_limits::max()); + + // minimum distance to lane changing start point + const double t_prepare = common_parameters.lane_change_prepare_duration; + const double a_min = lane_change_parameters_->min_longitudinal_acc; + const double min_dist_to_lane_changing_start = std::max( + current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); + + LaneChangeTargetObjectIndices filtered_obj_indices; + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto & obj_velocity_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + const auto extended_object = + utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + + // ignore specific object types + if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) { + continue; + } + + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + + // calc distance from the current ego position + double max_dist_ego_to_obj = std::numeric_limits::lowest(); + double min_dist_ego_to_obj = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const double dist_ego_to_obj = + motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); + min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); + } + + // ignore static object that are behind the ego vehicle + if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { + continue; + } + + // check if the object intersects with target lanes + if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { + // ignore static parked object that are in front of the ego vehicle in target lanes + bool is_parked_object = + utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0); + if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { + continue; + } + + filtered_obj_indices.target_lane.push_back(i); + continue; + } + + // check if the object intersects with target backward lanes + if ( + target_backward_polygon && + boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } + + // check if the object intersects with current lanes + if ( + current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && + max_dist_ego_to_obj >= 0.0) { + // check only the objects that are in front of the ego vehicle + filtered_obj_indices.current_lane.push_back(i); + continue; + } + + // ignore all objects that are behind the ego vehicle and not on the current and target + // lanes + if (max_dist_ego_to_obj < 0.0) { + continue; + } + + filtered_obj_indices.other_lane.push_back(i); + } + + return filtered_obj_indices; +} + PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -644,30 +839,14 @@ 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 longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; - const auto min_longitudinal_acc = - std::max(common_parameters.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto max_longitudinal_acc = - std::min(common_parameters.max_acc, lane_change_parameters_->max_longitudinal_acc); // get velocity - const auto current_velocity = getEgoTwist().linear.x; - - // compute maximum longitudinal deceleration and acceleration - const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, - &min_longitudinal_acc, &common_parameters]() { - const double min_a = (minimum_lane_changing_velocity - current_velocity) / - common_parameters.lane_change_prepare_duration; - return std::clamp( - min_a, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); - }); - const auto maximum_acceleration = utils::lane_change::calcMaximumAcceleration( - prev_module_path_, getEgoPose(), current_velocity, max_longitudinal_acc, common_parameters); + const auto current_velocity = getEgoVelocity(); // get sampling acceleration values - const auto longitudinal_acc_sampling_values = utils::lane_change::getAccelerationValues( - maximum_deceleration, maximum_acceleration, longitudinal_acc_sampling_num); + const auto longitudinal_acc_sampling_values = + sampleLongitudinalAccValues(current_lanes, target_lanes); const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); @@ -679,21 +858,13 @@ bool NormalLaneChange::getLaneChangePaths( const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto arc_position_from_target = - lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); - const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds( - route_handler, current_lanes, target_lanes, arc_position_from_target.distance); - - const auto target_neighbor_lanelets = - utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type_); + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); - const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( - target_neighbor_lanelets, 0, std::numeric_limits::max()); const auto target_neighbor_preferred_lane_poly_2d = - lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); + utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); const auto target_objects = getTargetObjects(current_lanes, target_lanes); @@ -772,8 +943,13 @@ bool NormalLaneChange::getLaneChangePaths( lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; const double s_goal = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + 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; if ( - s_start + lane_changing_length + common_parameters.lane_change_finish_judge_buffer + + s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); @@ -793,7 +969,16 @@ bool NormalLaneChange::getLaneChangePaths( const lanelet::BasicPoint2d lc_start_point( prepare_segment.points.back().point.pose.position.x, prepare_segment.points.back().point.pose.position.y); - if (!boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d)) { + + const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( + target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + if (!is_valid_start_point) { // lane changing points are not inside of the target preferred lanes or its neighbors continue; } @@ -855,8 +1040,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, object_debug_); + *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); if (is_safe) { return true; @@ -869,7 +1053,6 @@ bool NormalLaneChange::getLaneChangePaths( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { - const auto & common_parameters = getCommonParam(); const auto & path = status_.lane_change_path; const auto & current_lanes = status_.current_lanes; const auto & target_lanes = status_.target_lanes; @@ -878,8 +1061,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; const auto safety_status = isLaneChangePathSafe( - path, target_objects, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1136,7 +1318,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const { PathSafetyStatus path_safety_status; @@ -1194,18 +1376,19 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj : collision_check_objects) { auto current_debug_data = assignDebugData(obj); current_debug_data.second.ego_predicted_path.push_back(debug_predicted_path); - const auto obj_predicted_paths = - utils::getPredictedPathFromObj(obj, lane_change_parameters_->use_all_predicted_path); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::safety_check::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, + if (!utils::path_safety_checker::checkCollision( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } } updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 5c92d7ced6036..5a039a98f2e52 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -51,8 +51,8 @@ void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = "side_shift."; // updateParam(parameters, ns + ..., ...); - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 8e169309dfb00..4e9680cee4d59 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -185,7 +185,7 @@ void SideShiftModule::updateData() } } - if (current_state_ != ModuleStatus::RUNNING) { + if (current_state_ != ModuleStatus::RUNNING && current_state_ != ModuleStatus::IDLE) { return; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index d6104480f1cc3..9d2da0d13f56b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -42,6 +42,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = @@ -77,6 +78,75 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // freespace planner general params + { + std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + node->declare_parameter(ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + node->declare_parameter(ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + node->declare_parameter(ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); + p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_planner_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_planner_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_planner_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_planner_common_parameters.maximum_turning_radius = std::max( + p.freespace_planner_common_parameters.maximum_turning_radius, + p.freespace_planner_common_parameters.minimum_turning_radius); + p.freespace_planner_common_parameters.turning_radius_size = + std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + } + // freespace planner search config + { + std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + node->declare_parameter(ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + node->declare_parameter(ns + "obstacle_threshold"); + } + // freespace planner astar + { + std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.only_behind_solutions = + node->declare_parameter(ns + "only_behind_solutions"); + p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + node->declare_parameter(ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); + } // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { @@ -99,12 +169,71 @@ void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = name_ + "."; - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&](const auto & m) { - m->updateModuleParams(p); - m->setInitialIsSimultaneousExecutableAsApprovedModule( - enable_simultaneous_execution_as_approved_module_); - m->setInitialIsSimultaneousExecutableAsCandidateModule( - enable_simultaneous_execution_as_candidate_module_); + std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { + if (!observer.expired()) { + const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); + if (start_planner_ptr) { + start_planner_ptr->updateModuleParams(p); + } + } }); } + +bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const +{ + if (observers_.empty()) { + return enable_simultaneous_execution_as_approved_module_; + } + + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return enable_simultaneous_execution_as_approved_module_; + } + + const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); + + // Currently simultaneous execution with other modules is not supported while backward driving + if (!start_planner_ptr->isBackFinished()) { + return false; + } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + + return enable_simultaneous_execution_as_approved_module_; + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); +} + +bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const +{ + if (observers_.empty()) { + return enable_simultaneous_execution_as_candidate_module_; + } + + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return enable_simultaneous_execution_as_candidate_module_; + } + + const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); + + // Currently simultaneous execution with other modules is not supported while backward driving + if (!start_planner_ptr->isBackFinished()) { + return false; + } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + + return enable_simultaneous_execution_as_candidate_module_; + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 84b974427005a..2dc2401571d9a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -15,9 +15,9 @@ #include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -58,6 +58,34 @@ StartPlannerModule::StartPlannerModule( if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } + + if (parameters_->enable_freespace_planner) { + freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); + freespace_planner_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + freespace_planner_timer_ = rclcpp::create_timer( + &node, clock_, freespace_planner_period_ns, + std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), + freespace_planner_timer_cb_group_); + } +} + +void StartPlannerModule::onFreespacePlannerTimer() +{ + if (!planner_data_) { + return; + } + + if (!planner_data_->costmap) { + return; + } + + const bool is_new_costmap = + (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + if (isStuck() && is_new_costmap) { + planFreespacePath(); + } } BehaviorModuleOutput StartPlannerModule::run() @@ -78,27 +106,28 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { + // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. + // Execute when current pose is near route start pose + const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + if ( + tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > + parameters_->th_arrived_distance) { + return false; + } + // Check if ego arrives at goal const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); - const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { return false; } - has_received_new_route_ = - !planner_data_->prev_route_id || - *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - if (current_state_ == ModuleStatus::RUNNING) { return true; } - if (!has_received_new_route_) { - return false; - } - const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) < parameters_->th_stopped_velocity; if (!is_stopped) { @@ -115,7 +144,8 @@ bool StartPlannerModule::isExecutionRequested() const const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); auto lanes = current_lanes; @@ -166,7 +196,9 @@ BehaviorModuleOutput StartPlannerModule::plan() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } if (isWaitingApproval()) { @@ -178,21 +210,11 @@ BehaviorModuleOutput StartPlannerModule::plan() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - // the path of getCurrent() is generated by generateStopPath() - const PathWithLaneId stop_path = getCurrentPath(); - output.path = std::make_shared(stop_path); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = status_.lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - - output.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = std::make_shared(stop_path); - path_reference_ = getPreviousModuleOutput().reference_path; + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() return output; } @@ -207,19 +229,14 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -230,10 +247,6 @@ BehaviorModuleOutput StartPlannerModule::plan() }); if (status_.back_finished) { - setIsSimultaneousExecutableAsApprovedModule( - initial_value_simultaneously_executable_as_approved_module_); - setIsSimultaneousExecutableAsCandidateModule( - initial_value_simultaneously_executable_as_candidate_module_); const double start_distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -247,8 +260,6 @@ BehaviorModuleOutput StartPlannerModule::plan() {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); } else { - setIsSimultaneousExecutableAsApprovedModule(false); - setIsSimultaneousExecutableAsCandidateModule(false); const double distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -269,23 +280,8 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } -std::shared_ptr StartPlannerModule::getCurrentPlanner() const -{ - for (const auto & planner : start_planners_) { - if (status_.planner_type == planner->getPlannerType()) { - return planner; - } - } - return nullptr; -} - PathWithLaneId StartPlannerModule::getFullPath() const { - const auto pull_out_planner = getCurrentPlanner(); - if (pull_out_planner == nullptr) { - return PathWithLaneId{}; - } - // combine partial pull out path PathWithLaneId pull_out_path; for (const auto & partial_path : status_.pull_out_path.partial_paths) { @@ -312,18 +308,20 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - path_reference_ = getPreviousModuleOutput().reference_path; clearWaitingApproval(); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - path_reference_ = getPreviousModuleOutput().reference_path; clearWaitingApproval(); - return generateStopOutput(); + const auto output = generateStopOutput(); + setDebugData(); // use status updated in generateStopOutput() + return output; } waitApproval(); @@ -331,13 +329,11 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes); + const auto drivable_lanes = generateDrivableLanes(stop_path); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, @@ -346,17 +342,14 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = expanded_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(stop_path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -367,10 +360,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() }); if (status_.back_finished) { - setIsSimultaneousExecutableAsApprovedModule( - initial_value_simultaneously_executable_as_approved_module_); - setIsSimultaneousExecutableAsCandidateModule( - initial_value_simultaneously_executable_as_candidate_module_); const double start_distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -383,8 +372,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); } else { - setIsSimultaneousExecutableAsApprovedModule(false); - setIsSimultaneousExecutableAsCandidateModule(false); const double distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -423,18 +410,12 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority) { - status_.is_safe = false; - status_.planner_type = PlannerType::NONE; - // check if start pose candidates are valid if (start_pose_candidates.empty()) { return; } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); @@ -446,7 +427,8 @@ void StartPlannerModule::planWithPriority( } // use current path if back is not needed if (status_.back_finished) { - status_.is_safe = true; + const std::lock_guard lock(mutex_); + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); @@ -465,10 +447,13 @@ void StartPlannerModule::planWithPriority( } // Update status variables with the next path information - status_.is_safe = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); + { + const std::lock_guard lock(mutex_); + status_.is_safe_static_objects = true; + status_.pull_out_path = *pull_out_path_next; + status_.pull_out_start_pose = pull_out_start_pose_next; + status_.planner_type = planner->getPlannerType(); + } return true; }; @@ -508,7 +493,19 @@ void StartPlannerModule::planWithPriority( } for (const auto & p : order_priority) { - if (is_safe_with_pose_planner(p.first, p.second)) break; + if (is_safe_with_pose_planner(p.first, p.second)) { + const std::lock_guard lock(mutex_); + // Set back_finished flag based on the current index + status_.back_finished = p.first == 0; + return; + } + } + + // not found safe path + if (status_.planner_type != PlannerType::FREESPACE) { + const std::lock_guard lock(mutex_); + status_.is_safe_static_objects = false; + status_.planner_type = PlannerType::NONE; } } @@ -523,10 +520,9 @@ PathWithLaneId StartPlannerModule::generateStopPath() const PathPointWithLaneId p{}; p.point.pose = pose; p.point.longitudinal_velocity_mps = 0.0; - lanelet::Lanelet closest_shoulder_lanelet; - lanelet::utils::query::getClosestLanelet( - status_.pull_out_lanes, pose, &closest_shoulder_lanelet); - p.lane_ids.push_back(closest_shoulder_lanelet.id()); + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(status_.pull_out_lanes, pose, &closest_lanelet); + p.lane_ids.push_back(closest_lanelet.id()); return p; }; @@ -534,21 +530,76 @@ PathWithLaneId StartPlannerModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(current_pose)); path.points.push_back(toPathPointWithLaneId(moved_pose)); - // generate drivable area - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes); - return path; } +lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId & path) const +{ + const auto & route_handler = planner_data_->route_handler; + const auto & lanelet_layer = route_handler->getLaneletMapPtr()->laneletLayer; + + std::vector lane_ids; + for (const auto & p : path.points) { + for (const auto & id : p.lane_ids) { + if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) { + continue; + } + if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { + lane_ids.push_back(id); + } + } + } + + lanelet::ConstLanelets path_lanes; + path_lanes.reserve(lane_ids.size()); + for (const auto & id : lane_ids) { + if (id != lanelet::InvalId) { + path_lanes.push_back(lanelet_layer.get(id)); + } + } + + return path_lanes; +} + +std::vector StartPlannerModule::generateDrivableLanes( + const PathWithLaneId & path) const +{ + const auto path_road_lanes = getPathRoadLanes(path); + + if (!path_road_lanes.empty()) { + return utils::generateDrivableLanesWithShoulderLanes( + getPathRoadLanes(path), status_.pull_out_lanes); + } + + // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes + std::vector drivable_lanes; + for (const auto & lane : status_.pull_out_lanes) { + DrivableLanes drivable_lane; + drivable_lane.right_lane = lane; + drivable_lane.left_lane = lane; + drivable_lanes.push_back(drivable_lane); + } + return drivable_lanes; +} + void StartPlannerModule::updatePullOutStatus() { - if (has_received_new_route_) { + const bool has_received_new_route = + !planner_data_->prev_route_id || + *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); + + if (has_received_new_route) { status_ = PullOutStatus(); } + // save pull out lanes which is generated using current pose before starting pull out + // (before approval) + status_.pull_out_lanes = start_planner_utils::getPullOutLanes( + planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route_ && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!has_received_new_route && !last_pull_out_start_update_time_ && !status_.back_finished) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -563,41 +614,16 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - status_.current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); - status_.pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - - // combine road and shoulder lanes - status_.lanes = - utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_out_lanes); - // search pull out start candidates backward std::vector start_pose_candidates = searchPullOutStartPoses(); planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); - if (!status_.is_safe) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); - status_.back_finished = true; // no need to drive backward - status_.pull_out_path.partial_paths.clear(); - status_.pull_out_path.partial_paths.push_back(generateStopPath()); - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; - } - checkBackFinished(); if (!status_.back_finished) { status_.backward_path = start_planner_utils::getBackwardPath( *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose, parameters_->backward_velocity); } - - // Update status - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.pull_out_lane_ids = utils::getIds(status_.pull_out_lanes); } std::vector StartPlannerModule::searchPullOutStartPoses() @@ -614,6 +640,13 @@ std::vector StartPlannerModule::searchPullOutStartPoses() status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // filter pull out lanes stop objects + const auto [pull_out_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, status_.pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_->th_moving_object_velocity); + // lateral shift to current_pose const double distance_from_center_line = arc_position_pose.distance; for (auto & p : backward_shoulder_path.points) { @@ -656,7 +689,7 @@ std::vector StartPlannerModule::searchPullOutStartPoses() } if (utils::checkCollisionBetweenFootprintAndObjects( - local_vehicle_footprint, *backed_pose, *(planner_data_->dynamic_object), + local_vehicle_footprint, *backed_pose, pull_out_lane_stop_objects, parameters_->collision_check_margin)) { break; // poses behind this has a collision, so break. } @@ -681,17 +714,26 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished) { + if (!status_.back_finished || !status_.is_safe_static_objects) { return false; } const auto current_pose = planner_data_->self_odometry->pose.pose; + if (status_.planner_type == PlannerType::FREESPACE) { + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; + } // check that ego has passed pull out end point - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto arclength_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose); const auto arclength_pull_out_end = - lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); + lanelet::utils::getArcCoordinates(current_lanes, status_.pull_out_path.end_pose); // offset to not finish the module before engage constexpr double offset = 0.1; @@ -748,6 +790,24 @@ bool StartPlannerModule::isStopped() return is_stopped; } +bool StartPlannerModule::isStuck() +{ + if (!isStopped()) { + return false; + } + + if (status_.planner_type == PlannerType::STOP) { + return true; + } + + // not found safe path + if (!status_.is_safe_static_objects) { + return true; + } + + return false; +} + bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); @@ -788,10 +848,17 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const if (path.points.empty()) { return {}; } - const auto closest_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - const auto lane_id = path.points.at(closest_idx).lane_ids.front(); - const auto lane = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id); - const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose); + + // calculate lateral offset from pull out target lane center line + lanelet::ConstLanelet closest_road_lane; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); + const double lateral_offset = + lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; @@ -881,20 +948,96 @@ bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const } // NOTE: this must be called after updatePullOutStatus(). This must be fixed. -BehaviorModuleOutput StartPlannerModule::generateStopOutput() const +BehaviorModuleOutput StartPlannerModule::generateStopOutput() { BehaviorModuleOutput output; - output.path = std::make_shared(generateStopPath()); + const PathWithLaneId stop_path = generateStopPath(); + output.path = std::make_shared(stop_path); - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = status_.lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + setDrivableAreaInfo(output); output.reference_path = getPreviousModuleOutput().reference_path; + + { + const std::lock_guard lock(mutex_); + status_.back_finished = true; + status_.planner_type = PlannerType::STOP; + status_.pull_out_path.partial_paths.clear(); + status_.pull_out_path.partial_paths.push_back(stop_path); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + status_.pull_out_start_pose = current_pose; + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; + } + + path_candidate_ = std::make_shared(stop_path); + path_reference_ = getPreviousModuleOutput().reference_path; + return output; } +bool StartPlannerModule::planFreespacePath() +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto & route_handler = planner_data_->route_handler; + + const double end_pose_search_start_distance = parameters_->end_pose_search_start_distance; + const double end_pose_search_end_distance = parameters_->end_pose_search_end_distance; + const double end_pose_search_interval = parameters_->end_pose_search_interval; + + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + + const double s_start = std::max(0.0, current_arc_coords.length + end_pose_search_start_distance); + const double s_end = current_arc_coords.length + end_pose_search_end_distance; + + auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), end_pose_search_interval); + + for (const auto & p : center_line_path.points) { + const Pose end_pose = p.point.pose; + freespace_planner_->setPlannerData(planner_data_); + auto freespace_path = freespace_planner_->plan(current_pose, end_pose); + + if (!freespace_path) { + continue; + } + + const std::lock_guard lock(mutex_); + status_.pull_out_path = *freespace_path; + status_.pull_out_start_pose = current_pose; + status_.planner_type = freespace_planner_->getPlannerType(); + status_.is_safe_static_objects = true; + status_.back_finished = true; + return true; + } + + return false; +} + +void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +{ + if (status_.planner_type == PlannerType::FREESPACE) { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + } else { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, generateDrivableLanes(*output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } +} + void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; @@ -921,8 +1064,8 @@ void StartPlannerModule::setDebugData() const const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 78ea16dbc3c05..fa661aef06681 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -260,21 +260,24 @@ boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo continue; } - if ( - (lane_attribute == "right" || lane_attribute == "left" || lane_attribute == "straight") && - dist_to_front_point < search_distance) { - // update map if necessary - if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { - desired_start_point_map_.emplace(lane_id, current_pose); + constexpr double stop_velocity_threshold = 0.1; + if (dist_to_front_point < search_distance) { + if ( + lane_attribute == "right" || lane_attribute == "left" || + (lane_attribute == "straight" && current_vel < stop_velocity_threshold)) { + // update map if necessary + if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { + desired_start_point_map_.emplace(lane_id, current_pose); + } + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = desired_start_point_map_.at(lane_id); + 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); + signal_queue.push(turn_signal_info); } - - TurnSignalInfo turn_signal_info{}; - turn_signal_info.desired_start_point = desired_start_point_map_.at(lane_id); - 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); - signal_queue.push(turn_signal_info); } } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d3c31d688557d..4cbfebef07a39 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -142,6 +142,59 @@ double calcSignedArcLengthToFirstNearestPoint( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } + +geometry_msgs::msg::Polygon createVehiclePolygon( + const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) +{ + const auto & i = vehicle_info; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0 + offset; + const auto & back_m = i.rear_overhang_m; + + geometry_msgs::msg::Polygon polygon{}; + + polygon.points.push_back(createPoint32(front_m, -width_m, 0.0)); + polygon.points.push_back(createPoint32(front_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-back_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-back_m, -width_m, 0.0)); + + return polygon; +} + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, + const geometry_msgs::msg::Polygon & base_polygon) +{ + Polygon2d one_step_polygon{}; + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p_front); + 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(p_back); + 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)); + } + } + + Polygon2d hull_polygon{}; + boost::geometry::convex_hull(one_step_polygon, hull_polygon); + boost::geometry::correct(hull_polygon); + + return hull_polygon; +} } // namespace bool isOnRight(const ObjectData & obj) @@ -640,9 +693,9 @@ void fillObjectMovingTime( const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); - const auto & object_vel = - object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const auto is_faster_than_threshold = object_vel > object_parameter.moving_speed_threshold; + const auto & object_twist = object_data.object.kinematics.initial_twist_with_covariance.twist; + const auto object_vel_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); + const auto is_faster_than_threshold = object_vel_norm > object_parameter.moving_speed_threshold; const auto id = object_data.object.object_id; const auto same_id_obj = std::find_if( @@ -725,7 +778,7 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) { - if (!parameters->use_constraints_for_decel) { + if (parameters->policy_deceleration == "reliable") { object_data.is_stoppable = true; return; } @@ -855,6 +908,10 @@ void filterTargetObjects( using lanelet::geometry::toArcCoordinates; using lanelet::utils::to2D; + if (data.current_lanelets.empty()) { + return; + } + const auto & rh = planner_data->route_handler; const auto & path_points = data.reference_path_rough.points; const auto & ego_pos = planner_data->self_odometry->pose.pose.position; @@ -988,18 +1045,27 @@ void filterTargetObjects( // calculate avoid_margin dynamically // NOTE: This calculation must be after calculating to_road_shoulder_distance. - const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const double min_safety_lateral_distance = - object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; - const auto max_allowable_lateral_distance = - o.to_road_shoulder_distance - parameters->road_shoulder_safety_margin - 0.5 * vehicle_width; + const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto soft_lateral_distance_limit = + o.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; + const auto hard_lateral_distance_limit = + o.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; const auto avoid_margin = [&]() -> boost::optional { - if (max_allowable_lateral_distance < min_safety_lateral_distance) { + // Step1. check avoidable or not. + if (hard_lateral_distance_limit < min_avoid_margin) { return boost::none; } - return std::min(max_allowable_lateral_distance, max_avoid_margin); + + // Step2. check if it should expand road shoulder margin. + if (soft_lateral_distance_limit < min_avoid_margin) { + return min_avoid_margin; + } + + // Step3. nominal case. avoid margin is limited by soft constraint. + return std::min(soft_lateral_distance_limit, max_avoid_margin); }(); if (!!avoid_margin) { @@ -1367,4 +1433,237 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } + +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (path.points.empty()) { + return {}; + } + + const auto & acceleration = parameters->max_acceleration; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path.points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = + std::max(initial_velocity + acceleration * t, parameters->min_slow_down_speed); + const double length = initial_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto & obj_velocity_norm = std::hypot( + extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths.at(i); + extended_object.predicted_paths.at(i).confidence = path.confidence; + + // create path + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths.at(i).path.emplace_back( + t, *obj_pose, obj_velocity_norm, obj_polygon); + } + } + } + + return extended_object; +} + +lanelet::ConstLanelets getAdjacentLane( + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & rh = planner_data->route_handler; + const auto & forward_distance = parameters->object_check_forward_distance; + const auto & backward_distance = parameters->safety_check_backward_distance; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(vehicle_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Satoshi Ota) + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance); + + lanelet::ConstLanelets lanes{}; + for (const auto & lane : ego_succeeding_lanes) { + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (!is_right_shift && opt_left_lane) { + lanes.push_back(opt_left_lane.get()); + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (is_right_shift && opt_right_lane) { + lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (is_right_shift && !right_opposite_lanes.empty()) { + lanes.push_back(right_opposite_lanes.front()); + } + } + + return lanes; +} + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & p = parameters; + const auto check_right_lanes = + (is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane); + const auto check_left_lanes = + (!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane); + + std::vector target_objects; + + const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { + std::for_each(objects.begin(), objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object.object, check_lanes)) { + target_objects.push_back(utils::avoidance::transform(object.object, p)); + } + }); + }; + + const auto unavoidable_objects = [&data]() { + ObjectDataArray ret; + std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { + if (!object.is_avoidable) { + ret.push_back(object); + } + }); + return ret; + }(); + + // check right lanes + if (check_right_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, true); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check left lanes + if (check_left_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, false); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check current lanes + if (p->check_current_lane) { + const auto check_lanes = data.current_lanelets; + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + return target_objects; +} + +std::pair separateObjectsByPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const AvoidancePlanningData & data, const std::shared_ptr & parameters, + DebugData & debug) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + double max_offset = 0.0; + for (const auto & object_parameter : parameters->object_parameters) { + const auto p = object_parameter.second; + const auto offset = + 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + max_offset = std::max(max_offset, offset); + } + + 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); + if (distance_from_ego > parameters->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); + } + } + + debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); + + 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)) { + other_objects.objects.push_back(object); + } else { + target_objects.objects.push_back(object); + } + }); + + return std::make_pair(target_objects, other_objects); +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 21e3580c1f19a..2b62d5e7c3b53 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -21,6 +21,8 @@ #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include "interpolation/linear_interpolation.hpp" +#include + #include namespace drivable_area_expansion @@ -33,7 +35,7 @@ void expandDrivableArea( { const auto uncrossable_lines = extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); - multilinestring_t uncrossable_lines_in_range; + multi_linestring_t uncrossable_lines_in_range; const auto & p = path.points.front().point.pose.position; for (const auto & line : uncrossable_lines) if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) @@ -61,7 +63,7 @@ Point convert_point(const point_t & p) } polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multipolygon_t & expansion_polygons) + const PathWithLaneId & path, const multi_polygon_t & expansion_polygons) { polygon_t original_da_poly; original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1); @@ -70,7 +72,7 @@ polygon_t createExpandedDrivableAreaPolygon( original_da_poly.outer().push_back(convert_point(*it)); original_da_poly.outer().push_back(original_da_poly.outer().front()); - multipolygon_t unions; + multi_polygon_t unions; auto expanded_da_poly = original_da_poly; for (const auto & p : expansion_polygons) { unions.clear(); @@ -81,59 +83,6 @@ polygon_t createExpandedDrivableAreaPolygon( return expanded_da_poly; } -std::array findLeftRightRanges( - const PathWithLaneId & path, const ring_t & expanded_drivable_area) -{ - const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { - return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; - }; - const auto is_left_of_path_start = [&](const point_t & p) { - return is_left_of_segment( - convert_point(path.points[0].point.pose.position), - convert_point(path.points[1].point.pose.position), p); - }; - const auto is_left_of_path_end = [&, size = path.points.size()](const point_t & p) { - return is_left_of_segment( - convert_point(path.points[size - 2].point.pose.position), - convert_point(path.points[size - 1].point.pose.position), p); - }; - const auto dist_to_path_start = [start = convert_point(path.points.front().point.pose.position)]( - const auto & p) { return boost::geometry::distance(start, p); }; - const auto dist_to_path_end = [end = convert_point(path.points.back().point.pose.position)]( - const auto & p) { return boost::geometry::distance(end, p); }; - - double min_start_dist = std::numeric_limits::max(); - auto start_transition = expanded_drivable_area.end(); - double min_end_dist = std::numeric_limits::max(); - auto end_transition = expanded_drivable_area.end(); - for (auto it = expanded_drivable_area.begin(); std::next(it) != expanded_drivable_area.end(); - ++it) { - if (is_left_of_path_start(*it) != is_left_of_path_start(*std::next(it))) { - const auto dist = dist_to_path_start(*it); - if (dist < min_start_dist) { - start_transition = it; - min_start_dist = dist; - } - } - if (is_left_of_path_end(*it) != is_left_of_path_end(*std::next(it))) { - const auto dist = dist_to_path_end(*it); - if (dist < min_end_dist) { - end_transition = it; - min_end_dist = dist; - } - } - } - const auto left_start = - is_left_of_path_start(*start_transition) ? start_transition : std::next(start_transition); - const auto right_start = - is_left_of_path_start(*start_transition) ? std::next(start_transition) : start_transition; - const auto left_end = - is_left_of_path_end(*end_transition) ? end_transition : std::next(end_transition); - const auto right_end = - is_left_of_path_end(*end_transition) ? std::next(end_transition) : end_transition; - return {left_start, left_end, right_start, right_end}; -} - void copy_z_over_arc_length( const std::vector & from, std::vector & to) { @@ -164,32 +113,165 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_ { const auto original_left_bound = path.left_bound; const auto original_right_bound = path.right_bound; + const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) { + return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0; + }; + // prepare delimiting lines: start and end of the original expanded drivable area + const auto start_segment = + segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())}; + const auto end_segment = + segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())}; + point_t start_segment_center; + boost::geometry::centroid(start_segment, start_segment_center); + const auto path_start_segment = + segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)}; + point_t end_segment_center; + boost::geometry::centroid(end_segment, end_segment_center); + const auto path_end_segment = + segment_t{convert_point(path.points.back().point.pose.position), end_segment_center}; + const auto segment_to_line_intersection = + [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional { + const auto line = Eigen::Hyperplane::Through(q1, q2); + const auto segment = Eigen::Hyperplane::Through(p1, p2); + const auto intersection = line.intersection(segment); + std::optional result; + const auto is_on_segment = + (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x() + : intersection.x() <= p1.x() && intersection.x() >= p2.x()) && + (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y() + : intersection.y() <= p1.y() && intersection.y() >= p2.y()); + if (is_on_segment) result = point_t{intersection.x(), intersection.y()}; + return result; + }; + // find intersection between the expanded drivable area and the delimiting lines + const auto & da = expanded_drivable_area.outer(); + struct Intersection + { + point_t intersection_point; + ring_t::const_iterator segment_it; + double distance = std::numeric_limits::max(); + explicit Intersection(ring_t::const_iterator it) : segment_it(it) {} + void update(const point_t & p, const ring_t::const_iterator & it, const double dist) + { + intersection_point = p; + segment_it = it; + distance = dist; + } + }; + Intersection start_left(da.end()); + Intersection end_left(da.end()); + Intersection start_right(da.end()); + Intersection end_right(da.end()); + for (auto it = da.begin(); it != da.end(); ++it) { + if (boost::geometry::distance(*it, start_segment.first) < 1e-3) + start_left.update(*it, it, 0.0); + else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) + start_right.update(*it, it, 0.0); + else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) + end_left.update(*it, it, 0.0); + else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) + end_right.update(*it, it, 0.0); + const auto inter_start = + std::next(it) == da.end() + ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second) + : segment_to_line_intersection( + *it, *std::next(it), start_segment.first, start_segment.second); + if (inter_start) { + const auto dist = boost::geometry::distance(*inter_start, path_start_segment); + const auto is_left_of_path_start = is_left_of_segment( + convert_point(path.points[0].point.pose.position), + convert_point(path.points[1].point.pose.position), *inter_start); + if (is_left_of_path_start && dist < start_left.distance) + start_left.update(*inter_start, it, dist); + else if (!is_left_of_path_start && dist < start_right.distance) + start_right.update(*inter_start, it, dist); + } + const auto inter_end = + std::next(it) == da.end() + ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second) + : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); + if (inter_end) { + const auto dist = boost::geometry::distance(*inter_end, path_end_segment); + const auto is_left_of_path_end = is_left_of_segment( + convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end); + if (is_left_of_path_end && dist < end_left.distance) + end_left.update(*inter_end, it, dist); + else if (!is_left_of_path_end && dist < end_right.distance) + end_right.update(*inter_end, it, dist); + } + } + if (start_left.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, start_segment.first) < + boost::geometry::distance(b, start_segment.first); + }); + start_left.update(*closest_it, closest_it, 0.0); + } + if (start_right.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, start_segment.second) < + boost::geometry::distance(b, start_segment.second); + }); + start_right.update(*closest_it, closest_it, 0.0); + } + if (end_left.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, end_segment.first) < + boost::geometry::distance(b, end_segment.first); + }); + end_left.update(*closest_it, closest_it, 0.0); + } + if (end_right.segment_it == da.end()) { + const auto closest_it = + std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { + return boost::geometry::distance(a, end_segment.second) < + boost::geometry::distance(b, end_segment.second); + }); + end_right.update(*closest_it, closest_it, 0.0); + } + + // extract the expanded left and right bound from the expanded drivable area path.left_bound.clear(); path.right_bound.clear(); - const auto begin = expanded_drivable_area.outer().begin(); - const auto end = std::prev(expanded_drivable_area.outer().end()); - const auto & [left_start, left_end, right_start, right_end] = - findLeftRightRanges(path, expanded_drivable_area.outer()); - // NOTE: clockwise ordering -> positive increment for left bound, negative for right bound - if (left_start < left_end) { - path.left_bound.reserve(std::distance(left_start, left_end)); - for (auto it = left_start; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it)); - } else { // loop back - path.left_bound.reserve(std::distance(left_start, end) + std::distance(begin, left_end)); - for (auto it = left_start; it != end; ++it) path.left_bound.push_back(convert_point(*it)); - for (auto it = begin; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it)); + path.left_bound.push_back(convert_point(start_left.intersection_point)); + path.right_bound.push_back(convert_point(start_right.intersection_point)); + if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it)) + path.right_bound.push_back(convert_point(*start_right.segment_it)); + if (start_left.segment_it < end_left.segment_it) { + for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it) + path.left_bound.push_back(convert_point(*it)); + } else { + for (auto it = std::next(start_left.segment_it); it < da.end(); ++it) + path.left_bound.push_back(convert_point(*it)); + for (auto it = da.begin(); it <= end_left.segment_it; ++it) + path.left_bound.push_back(convert_point(*it)); } - if (right_start > right_end) { - path.right_bound.reserve(std::distance(right_end, right_start)); - for (auto it = right_start; it >= right_end; --it) + if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it)) + path.left_bound.push_back(convert_point(end_left.intersection_point)); + if (start_right.segment_it < end_right.segment_it) { + for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it) + path.right_bound.push_back(convert_point(*it)); + for (auto it = std::prev(da.end()); it > end_right.segment_it; --it) + path.right_bound.push_back(convert_point(*it)); + } else { + for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it) path.right_bound.push_back(convert_point(*it)); - } else { // loop back - path.right_bound.reserve(std::distance(begin, right_start) + std::distance(right_end, end)); - for (auto it = right_start; it >= begin; --it) path.right_bound.push_back(convert_point(*it)); - for (auto it = end - 1; it >= right_end; --it) path.right_bound.push_back(convert_point(*it)); } + if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it))) + path.right_bound.push_back(convert_point(end_right.intersection_point)); + // remove possible duplicated points + const auto point_cmp = [](const auto & p1, const auto & p2) { + return p1.x == p2.x && p1.y == p2.y; + }; + path.left_bound.erase( + std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); + path.right_bound.erase( + std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), + path.right_bound.end()); copy_z_over_arc_length(original_left_bound, path.left_bound); copy_z_over_arc_length(original_right_bound, path.right_bound); } - } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp index 79e15bf4e68d7..035cc579d2a82 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp @@ -22,10 +22,10 @@ namespace drivable_area_expansion double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multilinestring_t & limit_lines) + const multi_linestring_t & limit_lines) { auto dist_limit = std::numeric_limits::max(); - multipoint_t intersections; + multi_point_t intersections; boost::geometry::intersection(expansion_polygon, limit_lines, intersections); for (const auto & p : intersections) dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); @@ -34,11 +34,11 @@ double calculateDistanceLimit( double calculateDistanceLimit( const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multipolygon_t & limit_polygons) + const multi_polygon_t & limit_polygons) { auto dist_limit = std::numeric_limits::max(); for (const auto & polygon : limit_polygons) { - multipoint_t intersections; + multi_point_t intersections; boost::geometry::intersection(expansion_polygon, polygon, intersections); for (const auto & p : intersections) dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); @@ -50,7 +50,7 @@ polygon_t createExpansionPolygon( const linestring_t & base_ls, const double dist, const bool is_left_side) { namespace strategy = boost::geometry::strategy::buffer; - multipolygon_t polygons; + multi_polygon_t polygons; // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer constexpr auto zero = 0.1; const auto left_dist = is_left_side ? dist : zero; @@ -66,7 +66,7 @@ std::array calculate_arc_length_range_and_distance( const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound, const bool is_left, const double path_length) { - multipoint_t intersections; + multi_point_t intersections; double expansion_dist = 0.0; double from_arc_length = std::numeric_limits::max(); double to_arc_length = std::numeric_limits::min(); @@ -93,7 +93,7 @@ std::array calculate_arc_length_range_and_distance( polygon_t create_compensation_polygon( const linestring_t & base_ls, const double compensation_dist, const bool is_left, - const multilinestring_t uncrossable_lines, const multipolygon_t & predicted_paths) + const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths) { polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left); double dist_limit = std::min( @@ -106,9 +106,9 @@ polygon_t create_compensation_polygon( return compensation_polygon; } -multipolygon_t createExpansionPolygons( - const PathWithLaneId & path, const multipolygon_t & path_footprints, - const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines, +multi_polygon_t createExpansionPolygons( + const PathWithLaneId & path, const multi_polygon_t & path_footprints, + const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, const DrivableAreaExpansionParameters & params) { linestring_t path_ls; @@ -118,9 +118,32 @@ multipolygon_t createExpansionPolygons( path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); + // extend the path linestring to the beginning and end of the drivable area + if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) { + const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]); + const auto right_proj_begin = + point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]); + const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls); + const auto right_ls_proj_begin = + point_to_linestring_projection(right_proj_begin.point, path_ls); + if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length) + path_ls.insert(path_ls.begin(), left_proj_begin.point); + else + path_ls.insert(path_ls.begin(), right_proj_begin.point); + const auto left_proj_end = + point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); + const auto right_proj_end = + point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); + const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls); + const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls); + if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length) + path_ls.push_back(left_proj_end.point); + else + path_ls.push_back(right_proj_end.point); + } const auto path_length = static_cast(boost::geometry::length(path_ls)); - multipolygon_t expansion_polygons; + multi_polygon_t expansion_polygons; for (const auto & footprint : path_footprints) { bool is_left = true; for (const auto & bound : {left_ls, right_ls}) { @@ -137,8 +160,9 @@ multipolygon_t createExpansionPolygons( : footprint_dist; auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); + const auto uncrossable_dist_limit = std::max( + 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - + params.avoid_linestring_dist); if (uncrossable_dist_limit < limited_dist) { limited_dist = uncrossable_dist_limit; if (params.compensate_uncrossable_lines) { @@ -160,12 +184,12 @@ multipolygon_t createExpansionPolygons( return expansion_polygons; } -multipolygon_t createExpansionLaneletPolygons( +multi_polygon_t createExpansionLaneletPolygons( const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multipolygon_t & path_footprints, const multipolygon_t & predicted_paths, + const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, const DrivableAreaExpansionParameters & params) { - multipolygon_t expansion_polygons; + multi_polygon_t expansion_polygons; lanelet::ConstLanelets candidates; const auto already_added = [&](const auto & ll) { return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) { diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 77c8b7faa27eb..0c31093a83c0e 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -39,11 +39,11 @@ polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -multipolygon_t createObjectFootprints( +multi_polygon_t createObjectFootprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { - multipolygon_t footprints; + multi_polygon_t footprints; if (params.avoid_dynamic_objects) { for (const auto & object : objects.objects) { const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; @@ -62,7 +62,7 @@ multipolygon_t createObjectFootprints( return footprints; } -multipolygon_t createPathFootprints( +multi_polygon_t createPathFootprints( const PathWithLaneId & path, const DrivableAreaExpansionParameters & params) { const auto left = params.ego_left_offset + params.ego_extra_left_offset; @@ -73,7 +73,7 @@ multipolygon_t createPathFootprints( base_footprint.outer() = { point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, point_t{front, left}}; - multipolygon_t footprints; + multi_polygon_t footprints; // skip the last footprint as its orientation is usually wrong footprints.reserve(path.points.size() - 1); double arc_length = 0.0; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index 39a69fbd74914..ded67c251f7ae 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -26,10 +26,10 @@ namespace drivable_area_expansion { -multilinestring_t extractUncrossableLines( +multi_linestring_t extractUncrossableLines( const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types) { - multilinestring_t lines; + multi_linestring_t lines; linestring_t line; for (const auto & ls : lanelet_map.lineStringLayer) { if (hasTypes(ls, uncrossable_types)) { diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 678e19e616f26..a5ccfc9517771 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -272,13 +273,10 @@ bool GeometricParallelParking::planPullOut( // get road center line path from pull_out end to goal, and combine after the second arc path const double s_start = getArcCoordinates(road_lanes, *end_pose).length; - const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; - const double road_lanes_length = std::accumulate( - road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { - return sum + lanelet::utils::getLaneletLength2d(lane); - }); - const bool goal_is_behind = s_goal < s_start; - const double s_end = goal_is_behind ? road_lanes_length : s_goal; + const auto path_end_info = start_planner_utils::calcEndArcLength( + s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; PathWithLaneId road_center_line_path = planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); @@ -305,7 +303,7 @@ bool GeometricParallelParking::planPullOut( paths.back().points = motion_utils::removeOverlapPoints(paths.back().points); // if the end point is the goal, set the velocity to 0 - if (!goal_is_behind) { + if (path_terminal_is_goal) { paths.back().points.back().point.longitudinal_velocity_mps = 0.0; } @@ -368,7 +366,8 @@ std::vector GeometricParallelParking::planOneTrial( { clearPaths(); - const auto common_params = planner_data_->parameters; + const auto & common_params = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); const double self_yaw = tf2::getYaw(start_pose.orientation); @@ -435,34 +434,49 @@ std::vector GeometricParallelParking::planOneTrial( PathWithLaneId path_turn_left = generateArcPath( Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward, is_forward); - path_turn_left.header = planner_data_->route_handler->getRouteHeader(); + path_turn_left.header = route_handler->getRouteHeader(); PathWithLaneId path_turn_right = generateArcPath( Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward, is_forward); - path_turn_right.header = planner_data_->route_handler->getRouteHeader(); + path_turn_right.header = route_handler->getRouteHeader(); - auto setLaneIds = [lanes](PathPointWithLaneId & p) { - for (const auto & lane : lanes) { - p.lane_ids.push_back(lane.id()); + // Need to add straight path to last right_turning for parking in parallel + if (std::abs(end_pose_offset) > 0) { + PathPointWithLaneId straight_point{}; + straight_point.point.pose = goal_pose; + // setLaneIds(straight_point); + path_turn_right.points.push_back(straight_point); + } + + // Populate lane ids for a given path. + // It checks if each point in the path is within a lane + // and if its ID hasn't been added yet, it appends the ID to the container. + std::vector path_lane_ids; + const auto populateLaneIds = [&](const auto & path) { + for (const auto & p : path.points) { + for (const auto & lane : lanes) { + if ( + lanelet::utils::isInLanelet(p.point.pose, lane) && + std::find(path_lane_ids.begin(), path_lane_ids.end(), lane.id()) == path_lane_ids.end()) { + path_lane_ids.push_back(lane.id()); + } + } } }; - auto setLaneIdsToPath = [setLaneIds](PathWithLaneId & path) { + populateLaneIds(path_turn_left); + populateLaneIds(path_turn_right); + + // Set lane ids to each point in a given path. + // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member. + const auto setLaneIdsToPath = [&](PathWithLaneId & path) { for (auto & p : path.points) { - setLaneIds(p); + p.lane_ids = path_lane_ids; } }; setLaneIdsToPath(path_turn_left); setLaneIdsToPath(path_turn_right); - // Need to add straight path to last right_turning for parking in parallel - if (std::abs(end_pose_offset) > 0) { - PathPointWithLaneId straight_point{}; - straight_point.point.pose = goal_pose; - setLaneIds(straight_point); - path_turn_right.points.push_back(straight_point); - } - // generate arc path vector paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index ebfac8a347134..f4aabb13b8916 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -25,7 +25,9 @@ namespace behavior_path_planner FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) -: PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} +: PullOverPlannerBase{node, parameters}, + velocity_{parameters.freespace_parking_velocity}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); @@ -58,8 +60,18 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) return {}; } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); + PathWithLaneId path = - utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); const auto reverse_indices = utils::getReversingIndices(path); std::vector partial_paths = utils::dividePath(path, reverse_indices); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 24c6d98ed09e5..6a5ccc827db29 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -45,7 +45,8 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) // prepare road nad shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); const auto shoulder_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 9123beca0d95c..1e57dcc319dc5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -26,6 +27,7 @@ namespace behavior_path_planner { using lane_departure_checker::LaneDepartureChecker; +using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; @@ -54,7 +56,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - auto lanes = utils::getExtendedCurrentLanes(planner_data_, backward_length, forward_length); + auto lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_length, forward_length, + /*forward_only_in_route*/ false); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); const auto goal_arc_coords = @@ -66,7 +70,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) parameters_.goal_search_interval); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -102,6 +107,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); + if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { + continue; + } + if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { continue; } @@ -144,7 +153,8 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, @@ -274,6 +284,20 @@ void GoalSearcher::createAreaPolygons(std::vector original_search_poses) } } +BasicPolygons2d GoalSearcher::getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const +{ + BasicPolygons2d area_polygons{}; + for (const auto & ll : lanes) { + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + for (const auto & area : reg_elem->noParkingAreas()) { + const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); + area_polygons.push_back(area_poly); + } + } + } + return area_polygons; +} + BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const { BasicPolygons2d area_polygons{}; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 8bf20b212d51d..2f5c1d9b05b7f 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -46,8 +46,9 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) 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::getExtendedCurrentLanes( + planner_data_, backward_search_length, forward_search_length, + /*forward_only_in_route*/ false); const auto shoulder_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index cc44e7f0e012d..19b6bd27ea07c 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -180,5 +180,27 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } + +bool isAllowedGoalModification( + const std::shared_ptr & route_handler, const bool left_side_parking) +{ + return route_handler->isAllowedGoalModification() || + checkOriginalGoalIsInShoulder(route_handler, left_side_parking); +} + +bool checkOriginalGoalIsInShoulder( + const std::shared_ptr & route_handler, const bool left_side_parking) +{ + const Pose & goal_pose = route_handler->getGoalPose(); + + const lanelet::ConstLanelets pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking); + 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); +} + } // namespace goal_planner_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 90f108267055a..f3a1260eecadc 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -17,9 +17,9 @@ #include "behavior_path_planner/parameters.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/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -61,24 +61,57 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMaximumAcceleration( - const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) +double calcMaximumLaneChangeLength( + const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const std::vector & shift_intervals, const double max_acc) { - if (path.points.empty()) { - return max_longitudinal_acc; + if (shift_intervals.empty()) { + return 0.0; } - const double & nearest_dist_threshold = params.ego_nearest_dist_threshold; - const double & nearest_yaw_threshold = params.ego_nearest_yaw_threshold; + 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; + + double vel = current_velocity; + double accumulated_length = 0.0; + for (const auto & shift_interval : shift_intervals) { + // prepare section + const double prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + vel = vel + max_acc * t_prepare; + + // lane changing section + const auto lat_acc = common_param.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 = + vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; + + accumulated_length += prepare_length + lane_changing_length + finish_judge_buffer; + vel = vel + max_acc * t_lane_changing; + } + accumulated_length += + common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double & max_path_velocity = - path.points.at(current_seg_idx).point.longitudinal_velocity_mps; - const double & prepare_duration = params.lane_change_prepare_duration; + return accumulated_length; +} - const double acc = (max_path_velocity - current_velocity) / prepare_duration; +double calcMinimumAcceleration( + const double current_velocity, const double min_longitudinal_acc, + const BehaviorPathPlannerParameters & params) +{ + const double min_lane_changing_velocity = params.minimum_lane_changing_velocity; + const double prepare_duration = params.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 prepare_duration = params.lane_change_prepare_duration; + const double acc = (current_max_velocity - current_velocity) / prepare_duration; return std::clamp(acc, 0.0, max_longitudinal_acc); } @@ -203,6 +236,18 @@ lanelet::ConstLanelets getTargetNeighborLanes( return neighbor_lanes; } +lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type) +{ + const auto target_neighbor_lanelets = + utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); + const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( + target_neighbor_lanelets, 0, std::numeric_limits::max()); + + return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); +} + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) @@ -640,9 +685,12 @@ std::string getStrDirection(const std::string & name, const Direction direction) } std::vector> getSortedLaneIds( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const double rough_shift_length) + const RouteHandler & route_handler, const Pose & current_pose, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) { + const auto rough_shift_length = + lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; + std::vector> sorted_lane_ids{}; sorted_lane_ids.reserve(target_lanes.size()); const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) { @@ -807,7 +855,9 @@ bool isParkedObject( using lanelet::geometry::distance2d; using lanelet::geometry::toArcCoordinates; - if (object.initial_twist.twist.linear.x > static_object_velocity_threshold) { + const double object_vel_norm = + std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y); + if (object_vel_norm > static_object_velocity_threshold) { return false; } @@ -986,7 +1036,9 @@ boost::optional getLeadingStaticObjectIdx( // ignore non-static object // TODO(shimizu): parametrize threshold - if (obj.initial_twist.twist.linear.x > 1.0) { + const double obj_vel_norm = + std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y); + if (obj_vel_norm > 1.0) { continue; } @@ -1030,88 +1082,6 @@ std::optional createPolygon( return lanelet::utils::to2D(polygon_3d).basicPolygon(); } -LaneChangeTargetObjectIndices filterObject( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, - const Pose & current_pose, const RouteHandler & route_handler, - const LaneChangeParameters & lane_change_parameter) -{ - // Guard - if (objects.objects.empty()) { - return {}; - } - - // Get path - const auto path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - - const auto current_polygon = - createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = - createPolygon(target_backward_lanes, 0.0, std::numeric_limits::max()); - - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & object = objects.objects.at(i); - const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; - - // ignore specific object types - if (!isTargetObjectType(object, lane_change_parameter)) { - continue; - } - - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - - // calc distance from the current ego position - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - for (const auto & polygon_p : obj_polygon.outer()) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - } - - // ignore static object that are behind the ego vehicle - if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { - continue; - } - - // check if the object intersects with target lanes - if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with target backward lanes - if ( - target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with current lanes - if ( - current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && - max_dist_ego_to_obj >= 0.0) { - // check only the objects that are in front of the ego vehicle - filtered_obj_indices.current_lane.push_back(i); - continue; - } - - // ignore all objects that are behind the ego vehicle and not on the current and target - // lanes - if (max_dist_ego_to_obj < 0.0) { - continue; - } - - filtered_obj_indices.other_lane.push_back(i); - } - - return filtered_obj_indices; -} - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters) @@ -1129,9 +1099,9 @@ ExtendedPredictedObject transform( const auto & prepare_duration = common_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; - const auto & obj_vel = extended_object.initial_twist.twist.linear.x; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; - const double obj_velocity = extended_object.initial_twist.twist.linear.x; + const double obj_vel_norm = std::hypot( + extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { @@ -1143,14 +1113,14 @@ ExtendedPredictedObject transform( // create path for (double t = start_time; t < end_time + std::numeric_limits::epsilon(); t += time_resolution) { - if (t < prepare_duration && obj_vel < velocity_threshold) { + if (t < prepare_duration && obj_vel_norm < velocity_threshold) { continue; } const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( - t, *obj_pose, obj_velocity, obj_polygon); + t, *obj_pose, obj_vel_norm, obj_polygon); } } } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp new file mode 100644 index 0000000000000..33afe0fe6642f --- /dev/null +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -0,0 +1,353 @@ +// 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/utils/path_safety_checker/objects_filtering.hpp" + +#include "behavior_path_planner/utils/utils.hpp" + +namespace behavior_path_planner::utils::path_safety_checker +{ + +PredictedObjects filterObjects( + const std::shared_ptr & objects, + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const geometry_msgs::msg::Point & current_pose, + const std::shared_ptr & params) +{ + // Guard + if (objects->objects.empty()) { + return PredictedObjects(); + } + + const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold; + const double object_check_forward_distance = params->object_check_forward_distance; + const double object_check_backward_distance = params->object_check_backward_distance; + const ObjectTypesToCheck & target_object_types = params->object_types_to_check; + + PredictedObjects filtered_objects; + + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + + filterObjectsByClass(filtered_objects, target_object_types); + + const auto path = route_handler->getCenterLinePath( + current_lanes, object_check_backward_distance, object_check_forward_distance); + + filterObjectsByPosition( + filtered_objects, path.points, current_pose, object_check_forward_distance, + object_check_backward_distance); + + return filtered_objects; +} + +PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +{ + return filterObjectsByVelocity(objects, -lim_v, lim_v); +} + +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, double min_v, double max_v) +{ + PredictedObjects filtered; + filtered.header = objects.header; + for (const auto & obj : objects.objects) { + const auto v_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + if (min_v < v_norm && v_norm < max_v) { + filtered.objects.push_back(obj); + } + } + return filtered; +} + +void filterObjectsByPosition( + PredictedObjects & objects, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance) +{ + // Create a new container to hold the filtered objects + PredictedObjects filtered; + filtered.header = objects.header; + + // Reserve space in the vector to avoid reallocations + filtered.objects.reserve(objects.objects.size()); + + for (const auto & obj : objects.objects) { + const double dist_ego_to_obj = motion_utils::calcSignedArcLength( + path_points, current_pose, obj.kinematics.initial_pose_with_covariance.pose.position); + + if (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance) { + filtered.objects.push_back(obj); + } + } + + // Replace the original objects with the filtered list + objects.objects = std::move(filtered.objects); + return; +} + +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + + PredictedObjects filtered_objects; + + for (auto & object : objects.objects) { + const auto t = utils::getHighestProbLabel(object.classification); + const auto is_object_type = + ((t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); + + // If the object type matches any of the target types, add it to the filtered list + if (is_object_type) { + filtered_objects.objects.push_back(object); + } + } + + // Replace the original objects with the filtered list + objects = std::move(filtered_objects); + + return; +} + +std::pair, std::vector> separateObjectIndicesByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return {}; + } + + std::vector target_indices; + std::vector other_indices; + + for (size_t i = 0; i < objects.objects.size(); i++) { + // create object polygon + const auto & obj = objects.objects.at(i); + // create object polygon + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + bool is_filtered_object = false; + for (const auto & llt : target_lanelets) { + // create lanelet polygon + const auto polygon2d = llt.polygon2d().basicPolygon(); + if (polygon2d.empty()) { + // no lanelet polygon + continue; + } + Polygon2d lanelet_polygon; + for (const auto & lanelet_point : polygon2d) { + lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); + } + lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); + // check the object does not intersect the lanelet + if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + target_indices.push_back(i); + is_filtered_object = true; + break; + } + } + + if (!is_filtered_object) { + other_indices.push_back(i); + } + } + + return std::make_pair(target_indices, other_indices); +} + +std::pair separateObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + const auto [target_indices, other_indices] = + separateObjectIndicesByLanelets(objects, target_lanelets); + + target_objects.objects.reserve(target_indices.size()); + other_objects.objects.reserve(other_indices.size()); + + for (const size_t i : target_indices) { + target_objects.objects.push_back(objects.objects.at(i)); + } + + for (const size_t i : other_indices) { + other_objects.objects.push_back(objects.objects.at(i)); + } + + return std::make_pair(target_objects, other_objects); +} + +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) +{ + if (!is_use_all_predicted_path) { + const auto max_confidence_path = std::max_element( + obj.predicted_paths.begin(), obj.predicted_paths.end(), + [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); + if (max_confidence_path != obj.predicted_paths.end()) { + return {*max_confidence_path}; + } + } + + return obj.predicted_paths; +} + +// TODO(Sugahara): should consider delay before departure +std::vector createPredictedPath( + const std::shared_ptr & ego_predicted_path_params, + const std::vector & path_points, + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx) +{ + if (path_points.empty()) { + return {}; + } + + const double min_slow_down_speed = ego_predicted_path_params->min_slow_speed; + const double acceleration = ego_predicted_path_params->acceleration; + const double time_horizon = ego_predicted_path_params->time_horizon; + const double time_resolution = ego_predicted_path_params->time_resolution; + + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path_points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = std::max(current_velocity + acceleration * t, min_slow_down_speed); + const double length = current_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const double safety_check_time_horizon, + const double safety_check_time_resolution) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths[i]; + extended_object.predicted_paths[i].confidence = path.confidence; + + // Create path based on time horizon and resolution + for (double t = 0.0; t < safety_check_time_horizon + 1e-3; t += safety_check_time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths[i].path.emplace_back( + t, *obj_pose, obj_velocity, obj_polygon); + } + } + } + + return extended_object; +} + +TargetObjectsOnLane createTargetObjectsOnLane( + const lanelet::ConstLanelets & current_lanes, const std::shared_ptr & route_handler, + const PredictedObjects & filtered_objects, const std::shared_ptr & params) +{ + const auto & object_lane_configuration = params->object_lane_configuration; + const bool include_opposite = params->include_opposite_lane; + const bool invert_opposite = params->invert_opposite_lane; + const double safety_check_time_horizon = params->safety_check_time_horizon; + const double safety_check_time_resolution = params->safety_check_time_resolution; + + lanelet::ConstLanelets all_left_lanelets; + lanelet::ConstLanelets all_right_lanelets; + + // Define lambda functions to update left and right lanelets + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_left_lanelets.insert(all_left_lanelets.end(), left_lanelets.begin(), left_lanelets.end()); + }; + + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto right_lanelets = route_handler->getAllRightSharedLinestringLanelets( + target_lane, include_opposite, invert_opposite); + all_right_lanelets.insert( + all_right_lanelets.end(), right_lanelets.begin(), right_lanelets.end()); + }; + + // Update left and right lanelets for each current lane + for (const auto & current_lane : current_lanes) { + update_left_lanelets(current_lane); + update_right_lanelets(current_lane); + } + + TargetObjectsOnLane target_objects_on_lane{}; + const auto append_objects_on_lane = [&](auto & lane_objects, const auto & check_lanes) { + std::for_each( + filtered_objects.objects.begin(), filtered_objects.objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object, check_lanes)) { + lane_objects.push_back( + transform(object, safety_check_time_horizon, safety_check_time_resolution)); + } + }); + }; + + // TODO(Sugahara): Consider shoulder and other lane objects + if (object_lane_configuration.check_current_lane) { + append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); + } + if (object_lane_configuration.check_left_lane) { + append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); + } + if (object_lane_configuration.check_right_lane) { + append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); + } + + return target_objects_on_lane; +} + +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp similarity index 84% rename from planning/behavior_path_planner/src/utils/safety_check.cpp rename to planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 01d10b2aeec1e..c6a68e108e7e1 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" -namespace behavior_path_planner::utils::safety_check +namespace behavior_path_planner::utils::path_safety_checker { void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { @@ -135,8 +135,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { const auto stoppingDistance = [](const auto vehicle_velocity, const auto vehicle_accel) { // compensate if user accidentally set the deceleration to some positive value @@ -145,23 +144,23 @@ double calcRssDistance( }; const double & reaction_time = - params.rear_vehicle_reaction_time + params.rear_vehicle_safety_time_margin; + rss_params.rear_vehicle_reaction_time + rss_params.rear_vehicle_safety_time_margin; const double front_object_stop_length = - stoppingDistance(front_object_velocity, front_object_deceleration); + stoppingDistance(front_object_velocity, rss_params.front_vehicle_deceleration); const double rear_object_stop_length = rear_object_velocity * reaction_time + - stoppingDistance(rear_object_velocity, rear_object_deceleration); + stoppingDistance(rear_object_velocity, rss_params.rear_vehicle_deceleration); return rear_object_stop_length - front_object_stop_length; } double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { - const double & lon_threshold = params.longitudinal_distance_min_threshold; + const double & lon_threshold = rss_params.longitudinal_distance_min_threshold; const auto max_vel = std::max(front_object_velocity, rear_object_velocity); - return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; + return rss_params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } boost::optional calcInterpolatedPoseWithVelocity( @@ -219,8 +218,8 @@ bool checkCollision( const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug) + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug) { debug.lerped_path.reserve(target_object_path.path.size()); @@ -267,16 +266,15 @@ bool checkCollision( : std::make_pair(ego_velocity, object_velocity); // compute rss dist - const auto rss_dist = calcRssDistance( - front_object_velocity, rear_object_velocity, front_object_deceleration, - rear_object_deceleration, common_parameters); + const auto rss_dist = + calcRssDistance(front_object_velocity, rear_object_velocity, rss_parameters); // minimum longitudinal length const auto min_lon_length = - calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = common_parameters.lateral_distance_max_threshold; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) @@ -303,4 +301,28 @@ bool checkCollision( return true; } -} // namespace behavior_path_planner::utils::safety_check + +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double collision_check_margin, + const double max_extra_stopping_margin) +{ + for (const auto & p : ego_path.points) { + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, + max_extra_stopping_margin); + + const auto ego_polygon = tier4_autoware_utils::toFootprint( + p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); + + for (const auto & object : dynamic_objects.objects) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const double distance = boost::geometry::distance(obj_polygon, ego_polygon); + if (distance < collision_check_margin) return true; + } + } + + return false; +} +} // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 653efff7058d2..9ca083af35c92 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -321,7 +321,7 @@ std::pair, std::vector> PathShifter::calcBaseLengths if (lateral_a_max < lateral_acc_limit_) { // no need to consider acceleration limit - RCLCPP_WARN_THROTTLE( + RCLCPP_DEBUG_THROTTLE( logger_, clock_, 3000, "No need to consider lateral acc limit. max: %f, limit: %f", lateral_a_max, lateral_acc_limit_); return getBaseLengthsWithoutAccelLimit(S, shift_length, v0, a, T, offset_back); diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index d2d70b0a46dfc..ec14a064bf51b 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -336,14 +336,28 @@ std::pair getPathTurnSignal( } PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets) { PathWithLaneId path; path.header = waypoints.header; - for (const auto & waypoint : waypoints.waypoints) { + for (size_t i = 0; i < waypoints.waypoints.size(); ++i) { + const auto & waypoint = waypoints.waypoints.at(i); PathPointWithLaneId point{}; point.point.pose = waypoint.pose.pose; - // point.lane_id = // todo + // put the lane that contain waypoints in lane_ids. + bool is_in_lanes = false; + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + is_in_lanes = true; + } + } + // If none of them corresponds, assign the previous lane_ids. + if (!is_in_lanes && i > 0) { + point.lane_ids = path.points.at(i - 1).lane_ids; + } + point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; path.points.push_back(point); } diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp new file mode 100644 index 0000000000000..ba34901d9df6a --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -0,0 +1,115 @@ +// 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/utils/start_planner/freespace_pull_out.hpp" + +#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner/utils/utils.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +FreespacePullOut::FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} +{ + freespace_planning_algorithms::VehicleShape vehicle_shape( + vehicle_info, parameters.vehicle_shape_margin); + if (parameters.freespace_planner_algorithm == "astar") { + use_back_ = parameters.astar_parameters.use_back; + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.freespace_planner_algorithm == "rrtstar") { + use_back_ = true; // no option for disabling back in rrtstar + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, + parameters.rrt_star_parameters); + } +} + +boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +{ + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const double forward_path_length = planner_data_->parameters.forward_path_length; + + planner_->setMap(*planner_data_->costmap); + + const bool found_path = planner_->makePlan(start_pose, end_pose); + if (!found_path) { + return {}; + } + + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // find candidate paths + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); + + const PathWithLaneId path = + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); + + const auto reverse_indices = utils::getReversingIndices(path); + std::vector partial_paths = utils::dividePath(path, reverse_indices); + + // remove points which are near the end pose + PathWithLaneId & last_path = partial_paths.back(); + const double th_end_distance = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + const size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_end_distance) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // push back generate road lane path between end pose and goal pose to last path + const auto & goal_pose = route_handler->getGoalPose(); + constexpr double offset_from_end_pose = 1.0; + const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); + const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; + + const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + + const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; + utils::correctDividedPathVelocity(partial_paths); + if (!path_terminal_is_goal) { + // not need to stop at terminal + last_path.points.back().point.longitudinal_velocity_mps = original_terminal_velocity; + } + + PullOutPath pull_out_path{}; + pull_out_path.partial_paths = partial_paths; + pull_out_path.start_pose = start_pose; + pull_out_path.end_pose = end_pose; + + return pull_out_path; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 9ab3101cdc4a5..45d36d51d69b4 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -39,11 +40,12 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p { PullOutPath output; - // combine road lane and shoulder lane + // combine road lane and pull out lane const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); auto lanes = road_lanes; for (const auto & pull_out_lane : pull_out_lanes) { @@ -65,23 +67,61 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p return {}; } - // collision check with objects in shoulder lanes + // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [shoulder_lane_objects, others] = - utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_out_lanes); + const auto [pull_out_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_out_lanes); + const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_out_lane_objects, parameters_.th_moving_object_velocity); + if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) { + 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) { output.partial_paths = planner_.getPaths(); + /* + Calculate the acceleration required to reach the forward parking velocity at the center of + the front path, assuming constant acceleration and deceleration. + v v + | | + | /\ | /\ + | / \ | / \ + | / \ | / \ + | / \ | / \ + |/________\_____ x |/________\______ t + 0 a_l/2 a_l 0 t_c 2*t_c + Notes: + a_l represents "arc_length_on_path_front" + t_c represents "time_to_center" + */ // insert stop velocity to first arc path end output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; + const double arc_length_on_first_arc_path = + motion_utils::calcArcLength(output.partial_paths.front().points); + const double time_to_center = arc_length_on_first_arc_path / (2 * velocity); + const double average_velocity = arc_length_on_first_arc_path / (time_to_center * 2); + const double average_acceleration = average_velocity / (time_to_center * 2); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(average_velocity, average_acceleration)); + const double arc_length_on_second_arc_path = + motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { - auto partial_paths = planner_.getPaths(); + const auto partial_paths = planner_.getPaths(); const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); + + // Calculate the acceleration required to reach the forward parking velocity at the center of + // the path, assuming constant acceleration and deceleration. + const double arc_length_on_path = motion_utils::calcArcLength(combined_path.points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index a34d9f536c682..7352821e140be 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -53,8 +54,8 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max()); - + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); // find candidate paths auto pull_out_paths = calcPullOutPaths( *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_); @@ -62,9 +63,11 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) return boost::none; } - // extract objects in shoulder lane for collision check + // extract stop objects in pull out lane for collision check const auto [pull_out_lane_objects, others] = - utils::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + 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) { @@ -109,7 +112,7 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path_start_to_end, pull_out_lane_objects, + vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects, parameters_.collision_check_margin)) { continue; } @@ -148,13 +151,10 @@ std::vector ShiftPullOut::calcPullOutPaths( // generate road lane reference path const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); - const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - - // if goal is behind start pose, use path with forward_path_length - const bool goal_is_behind = arc_position_goal.length < s_start; - const double s_forward_length = s_start + forward_path_length; - const double s_end = - goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; constexpr double RESAMPLE_INTERVAL = 1.0; PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( @@ -163,9 +163,12 @@ std::vector ShiftPullOut::calcPullOutPaths( // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { PullOutPath non_shifted_path{}; + // In non_shifted_path, to minimize safety checks, 0 is assigned to prevent the predicted_path + // of the ego vehicle from becoming too large. non_shifted_path.partial_paths.push_back(road_lane_reference_path); non_shifted_path.start_pose = start_pose; non_shifted_path.end_pose = start_pose; + non_shifted_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(0, 0)); return non_shifted_path; }); @@ -216,14 +219,6 @@ std::vector ShiftPullOut::calcPullOutPaths( const double before_shifted_pull_out_distance = std::max(pull_out_distance, pull_out_distance_converted); - // check has enough distance - const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); - if (!hasEnoughDistance( - before_shifted_pull_out_distance, road_lanes, start_pose, is_in_goal_route_section, - goal_pose)) { - continue; - } - // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) { candidate_paths.push_back(non_shifted_path); @@ -282,7 +277,7 @@ std::vector ShiftPullOut::calcPullOutPaths( } } // if the end point is the goal, set the velocity to 0 - if (!goal_is_behind) { + if (path_terminal_is_goal) { shifted_path.path.points.back().point.longitudinal_velocity_mps = 0.0; } @@ -291,6 +286,8 @@ std::vector ShiftPullOut::calcPullOutPaths( candidate_path.partial_paths.push_back(shifted_path.path); candidate_path.start_pose = shift_line.start; candidate_path.end_pose = shift_line.end; + candidate_path.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(terminal_velocity, longitudinal_acc)); candidate_paths.push_back(candidate_path); } @@ -319,25 +316,6 @@ double ShiftPullOut::calcPullOutLongitudinalDistance( return min_pull_out_distance; } -bool ShiftPullOut::hasEnoughDistance( - const double pull_out_total_distance, const lanelet::ConstLanelets & road_lanes, - const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose) -{ - // the goal is far so current_lanes do not include goal's lane - if (pull_out_total_distance > utils::getDistanceToEndOfLane(current_pose, road_lanes)) { - return false; - } - - // current_lanes include goal's lane - if ( - is_in_goal_route_section && - pull_out_total_distance > utils::getSignedDistance(current_pose, goal_pose, road_lanes)) { - return false; - } - - return true; -} - double ShiftPullOut::calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr) { diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 2a9ab676211fb..9ee0491ca017e 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -114,6 +114,30 @@ lanelet::ConstLanelets getPullOutLanes( // pull out from road lane return utils::getExtendedCurrentLanes( - planner_data, backward_length, /*forward_length*/ std::numeric_limits::max()); + planner_data, backward_length, + /*forward_length*/ std::numeric_limits::max(), + /*forward_only_in_route*/ true); } + +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose) +{ + const double s_forward_length = s_start + forward_path_length; + // use forward length if the goal pose is not in the lanelets + if (!utils::isInLanelets(goal_pose, road_lanes)) { + return {s_forward_length, false}; + } + + const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + + // If the goal is behind the start or beyond the forward length, use forward length. + if (s_goal < s_start || s_goal >= s_forward_length) { + return {s_forward_length, false}; + } + + // path end is goal + return {s_goal, true}; +} + } // namespace behavior_path_planner::start_planner_utils diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index dcab96069a0a8..eb2859ff700af 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -81,28 +81,33 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { - size_t closest_idx = motion_utils::findNearestSegmentIndex(points, target_point); - double min_lateral_dist = - std::fabs(motion_utils::calcLateralOffset(points, target_point, closest_idx)); - + 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)); - if (lon_dist < 0.0 || segment_length < lon_dist) { - continue; - } - - const double lat_dist = - std::fabs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return tier4_autoware_utils::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 std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + }(); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; min_lateral_dist = lat_dist; } } - return closest_idx; + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point); } bool checkHasSameLane( @@ -113,6 +118,12 @@ bool checkHasSameLane( const auto has_same = [&](const auto & ll) { return ll.id() == target_lane.id(); }; return std::find_if(lanelets.begin(), lanelets.end(), has_same) != lanelets.end(); } + +bool isSamePoint(const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2) +{ + constexpr double epsilon = 1e-3; + return std::abs(point1.x - point2.x) < epsilon && std::abs(point1.y - point2.y) < epsilon; +} } // namespace namespace behavior_path_planner::utils @@ -283,43 +294,88 @@ std::vector convertToGeometryPoints( return points; } +// NOTE: See the PR's figure. https://github.com/autowarefoundation/autoware.universe/pull/2880 std::vector concatenateTwoPolygons( const std::vector & front_polygon, const std::vector & back_polygon) { + const auto make_unique_polygon = [&](const auto & polygon) { + std::vector unique_polygon; + for (const auto & point : polygon) { + if (!unique_polygon.empty() && isSamePoint(unique_polygon.back().point, point.point)) { + continue; + } + unique_polygon.push_back(point); + } + return unique_polygon; + }; + const auto unique_front_polygon = make_unique_polygon(front_polygon); + const auto unique_back_polygon = make_unique_polygon(back_polygon); + // At first, the front polygon is the outside polygon bool is_front_polygon_outside = true; - size_t outside_idx = 0; + size_t before_outside_idx = 0; const auto get_out_poly = [&]() { - return is_front_polygon_outside ? front_polygon : back_polygon; + return is_front_polygon_outside ? unique_front_polygon : unique_back_polygon; }; const auto get_in_poly = [&]() { - return is_front_polygon_outside ? back_polygon : front_polygon; + return is_front_polygon_outside ? unique_back_polygon : unique_front_polygon; }; + // NOTE: Polygon points is assumed to be clock-wise. std::vector concatenated_polygon; - while (rclcpp::ok()) { - concatenated_polygon.push_back(get_out_poly().at(outside_idx)); - if (outside_idx == get_out_poly().size() - 1) { + // NOTE: Maximum number of loop is set to avoid infinity loop calculation just in case. + const size_t max_loop_num = (unique_front_polygon.size() + unique_back_polygon.size()) * 2; + for (size_t loop_idx = 0; loop_idx < max_loop_num; ++loop_idx) { + concatenated_polygon.push_back(get_out_poly().at(before_outside_idx)); + if (before_outside_idx == get_out_poly().size() - 1) { break; } - const size_t curr_idx = outside_idx; - const size_t next_idx = outside_idx + 1; + const size_t curr_idx = before_outside_idx; + const size_t next_idx = before_outside_idx + 1; + // NOTE: Two polygons may have two intersection points. Therefore the closest intersection + // point is used. + std::optional closest_idx = std::nullopt; + double min_dist_to_intersection = std::numeric_limits::max(); + PolygonPoint closest_intersect_point; for (size_t i = 0; i < get_in_poly().size() - 1; ++i) { const auto intersection = tier4_autoware_utils::intersect( get_out_poly().at(curr_idx).point, get_out_poly().at(next_idx).point, get_in_poly().at(i).point, get_in_poly().at(i + 1).point); - if (intersection) { - const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0}; - concatenated_polygon.push_back(intersect_point); + if (!intersection) { + continue; + } + if ( + isSamePoint(get_out_poly().at(curr_idx).point, get_in_poly().at(i).point) || + isSamePoint(get_out_poly().at(curr_idx).point, get_in_poly().at(i + 1).point) || + isSamePoint(get_out_poly().at(next_idx).point, get_in_poly().at(i).point) || + isSamePoint(get_out_poly().at(next_idx).point, get_in_poly().at(i + 1).point)) { + // NOTE: If the segments shares one point, the while loop will not end. + continue; + } - is_front_polygon_outside = !is_front_polygon_outside; - outside_idx = i; - break; + const auto intersect_point = PolygonPoint{*intersection, 0, 0.0, 0.0}; + const double dist_to_intersection = + tier4_autoware_utils::calcDistance2d(get_out_poly().at(curr_idx).point, *intersection); + if (dist_to_intersection < min_dist_to_intersection) { + closest_idx = i; + min_dist_to_intersection = dist_to_intersection; + closest_intersect_point = intersect_point; } } - outside_idx += 1; + + if (closest_idx) { + before_outside_idx = *closest_idx; + concatenated_polygon.push_back(closest_intersect_point); + is_front_polygon_outside = !is_front_polygon_outside; + } + + before_outside_idx += 1; + + if (loop_idx == max_loop_num - 1) { + return front_polygon; + } } return concatenated_polygon; @@ -655,73 +711,6 @@ double calcLongitudinalDistanceFromEgoToObjects( return min_distance; } -std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return {}; - } - - std::vector target_indices; - std::vector other_indices; - - for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); - bool is_filtered_object = false; - for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { - target_indices.push_back(i); - is_filtered_object = true; - break; - } - } - - if (!is_filtered_object) { - other_indices.push_back(i); - } - } - - return std::make_pair(target_indices, other_indices); -} - -std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) -{ - PredictedObjects target_objects; - PredictedObjects other_objects; - - const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); - - target_objects.objects.reserve(target_indices.size()); - other_objects.objects.reserve(other_indices.size()); - - for (const size_t i : target_indices) { - target_objects.objects.push_back(objects.objects.at(i)); - } - - for (const size_t i : other_indices) { - other_objects.objects.push_back(objects.objects.at(i)); - } - - return std::make_pair(target_objects, other_objects); -} - std::vector calcObjectsDistanceToPath( const PredictedObjects & objects, const PathWithLaneId & ego_path) { @@ -1212,6 +1201,16 @@ std::vector generateDrivableLanesWithShoulderLanes( return drivable_lanes; } +std::vector getNonOverlappingExpandedLanes( + PathWithLaneId & path, const std::vector & lanes, + const DrivableAreaExpansionParameters & parameters) +{ + const auto shorten_lanes = utils::cutOverlappedLanes(path, lanes); + return utils::expandLanelets( + shorten_lanes, parameters.drivable_area_left_bound_offset, + parameters.drivable_area_right_bound_offset, parameters.drivable_area_types_to_skip); +} + boost::optional getOverlappedLaneletId(const std::vector & lanes) { auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { @@ -1224,6 +1223,7 @@ boost::optional getOverlappedLaneletId(const std::vector boost::geometry::intersection( lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon(), intersections); + // if only one point intersects, it is assumed not to be overlapped if (intersections.size() > 1) { return true; @@ -1258,37 +1258,74 @@ boost::optional getOverlappedLaneletId(const std::vector std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes) { - const auto overlapped_lanelet_id = getOverlappedLaneletId(lanes); - if (!overlapped_lanelet_id) { + const auto overlapped_lanelet_idx = getOverlappedLaneletId(lanes); + if (!overlapped_lanelet_idx) { return lanes; } - const std::vector shorten_lanes{ - lanes.begin(), lanes.begin() + *overlapped_lanelet_id}; + std::vector shorten_lanes{lanes.begin(), lanes.begin() + *overlapped_lanelet_idx}; const auto shorten_lanelets = utils::transformToLanelets(shorten_lanes); - // create removed lanelets - std::vector removed_lane_ids; - for (size_t i = *overlapped_lanelet_id; i < lanes.size(); ++i) { - const auto target_lanelets = utils::transformToLanelets(lanes.at(i)); - for (const auto & target_lanelet : target_lanelets) { - // if target lane is inside of the shorten lanelets, we do not remove it - if (checkHasSameLane(shorten_lanelets, target_lanelet)) { - continue; + const auto original_points = path.points; + + path.points.clear(); + + const auto has_same_id_lane = [](const auto & lanelet, const auto & p) { + return std::any_of(p.lane_ids.begin(), p.lane_ids.end(), [&lanelet](const auto id) { + return lanelet.id() == id; + }); + }; + + const auto has_same_id_lanes = [&has_same_id_lane](const auto & lanelets, const auto & p) { + return std::any_of( + lanelets.begin(), lanelets.end(), + [&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); + } + + // check left lane + if (has_same_id_lane(lanes.left_lane, original_points.at(i))) { + start_point_idx = std::min(start_point_idx, i); + } + + // check middle lanes + if (has_same_id_lanes(lanes.middle_lanes, original_points.at(i))) { + start_point_idx = std::min(start_point_idx, i); } - removed_lane_ids.push_back(target_lanelet.id()); } } - for (size_t i = 0; i < path.points.size(); ++i) { - const auto & lane_ids = path.points.at(i).lane_ids; - for (const auto & lane_id : lane_ids) { - if ( - std::find(removed_lane_ids.begin(), removed_lane_ids.end(), lane_id) != - removed_lane_ids.end()) { - path.points.erase(path.points.begin() + i, path.points.end()); - return shorten_lanes; + // 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))) { + 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; } } @@ -1604,6 +1641,10 @@ std::vector calcBound( const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto is_clockwise_polygon = + boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { return p.id() == bound.front().id(); }); @@ -1619,7 +1660,8 @@ std::vector calcBound( // extract line strings between start_idx and end_idx. const size_t start_idx = std::distance(polygon.begin(), start_itr); const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : extract_bound_from_polygon(polygon, start_idx, end_idx, is_left)) { + for (const auto & point : + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { output_points.push_back(point); } @@ -1753,8 +1795,22 @@ void makeBoundLongitudinallyMonotonic( continue; } - for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { - set_orientation(ret, j, getPose(path_points.at(i)).orientation); + if (i + 1 == path_points.size()) { + for (size_t j = intersect_idx.get(); j < bound_with_pose.size(); j++) { + if (j + 1 == bound_with_pose.size()) { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j - 1).position, bound_with_pose.at(j).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } else { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j).position, bound_with_pose.at(j + 1).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } + } + } else { + for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { + set_orientation(ret, j, getPose(path_points.at(i)).orientation); + } } constexpr size_t OVERLAP_CHECK_NUM = 3; @@ -2592,25 +2648,6 @@ std::vector expandLanelets( return expanded_drivable_lanes; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) -{ - return filterObjectsByVelocity(objects, -lim_v, lim_v); -} - -PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) -{ - PredictedObjects filtered; - filtered.header = objects.header; - for (const auto & obj : objects.objects) { - const auto v = std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x); - if (min_v < v && v < max_v) { - filtered.objects.push_back(obj); - } - } - return filtered; -} - PathWithLaneId getCenterLinePathFromRootLanelet( const lanelet::ConstLanelet & root_lanelet, const std::shared_ptr & planner_data) @@ -2767,10 +2804,6 @@ BehaviorModuleOutput getReferencePath( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); - // for old architecture - generateDrivableArea( - reference_path, expanded_lanes, false, false, p.vehicle_length, planner_data); - BehaviorModuleOutput output; output.path = std::make_shared(reference_path); output.reference_path = std::make_shared(reference_path); @@ -2854,7 +2887,14 @@ lanelet::ConstLanelets extendNextLane( // Add next lane const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); if (!next_lanes.empty()) { - extended_lanes.push_back(next_lanes.front()); + // use the next lane in route if it exists + auto target_next_lane = next_lanes.front(); + for (const auto & next_lane : next_lanes) { + if (route_handler->isRouteLanelet(next_lane)) { + target_next_lane = next_lane; + } + } + extended_lanes.push_back(target_next_lane); } return extended_lanes; @@ -2868,9 +2908,15 @@ lanelet::ConstLanelets extendPrevLane( // Add previous lane const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); if (!prev_lanes.empty()) { - extended_lanes.insert(extended_lanes.begin(), prev_lanes.front()); + // use the previous lane in route if it exists + auto target_prev_lane = prev_lanes.front(); + for (const auto & prev_lane : prev_lanes) { + if (route_handler->isRouteLanelet(prev_lane)) { + target_prev_lane = prev_lane; + } + } + extended_lanes.insert(extended_lanes.begin(), target_prev_lane); } - return extended_lanes; } @@ -2885,26 +2931,24 @@ lanelet::ConstLanelets extendLanes( lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, - const double forward_length) + const double forward_length, const bool forward_only_in_route) { auto lanes = getCurrentLanes(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; - const auto is_loop = [&](const auto & target_lane) { - auto it = std::find_if(lanes.begin(), lanes.end(), [&](const lanelet::ConstLanelet & lane) { - return lane.id() == target_lane.id(); - }); - - return it != lanes.end(); - }; - while (backward_length_sum < backward_length) { auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); - - if (extended_lanes.empty() || is_loop(extended_lanes.front())) { + 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; } @@ -2918,8 +2962,13 @@ lanelet::ConstLanelets getExtendedCurrentLanes( while (forward_length_sum < forward_length) { auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); - - if (extended_lanes.empty() || is_loop(extended_lanes.back())) { + 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; } @@ -2928,6 +2977,16 @@ lanelet::ConstLanelets getExtendedCurrentLanes( } 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; } @@ -2960,21 +3019,6 @@ lanelet::ConstLanelets calcLaneAroundPose( return current_lanes; } -std::vector getPredictedPathFromObj( - const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) -{ - if (!is_use_all_predicted_path) { - const auto max_confidence_path = std::max_element( - obj.predicted_paths.begin(), obj.predicted_paths.end(), - [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); - if (max_confidence_path != obj.predicted_paths.end()) { - return {*max_confidence_path}; - } - } - - return obj.predicted_paths; -} - bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) { // We need at least three points to compute relative angle @@ -3156,9 +3200,11 @@ std::vector combineDrivableLanes( } // NOTE: If original_drivable_lanes_vec is shorter than new_drivable_lanes_vec, push back remained // new_drivable_lanes_vec. - updated_drivable_lanes_vec.insert( - updated_drivable_lanes_vec.end(), new_drivable_lanes_vec.begin() + new_drivable_lanes_idx + 1, - new_drivable_lanes_vec.end()); + if (new_drivable_lanes_idx + 1 < new_drivable_lanes_vec.size()) { + updated_drivable_lanes_vec.insert( + updated_drivable_lanes_vec.end(), new_drivable_lanes_vec.begin() + new_drivable_lanes_idx + 1, + new_drivable_lanes_vec.end()); + } return updated_drivable_lanes_vec; } @@ -3254,4 +3300,21 @@ void extractObstaclesFromDrivableArea( bound = drivable_area_processing::updateBoundary(bound, unique_polygons); } } + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes) +{ + lanelet::ConstLanelets combined_lanes = base_lanes; + for (const auto & added_lane : added_lanes) { + const auto it = std::find_if( + combined_lanes.begin(), combined_lanes.end(), + [&added_lane](const lanelet::ConstLanelet & lane) { return lane.id() == added_lane.id(); }); + if (it == combined_lanes.end()) { + combined_lanes.push_back(added_lane); + } + } + + return combined_lanes; +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 7c4967be14bb8..605b3b96ee9b9 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -165,13 +165,13 @@ TEST(DrivableAreaExpansionProjection, SubLinestring) for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); } { - // arc lengths equal to existing point: sublinestring with same points + // arc lengths equal to existing point: sub-linestring with same points const auto sub = sub_linestring(ls, 1.0, 5.0); ASSERT_EQ(ls.size() - 2lu, sub.size()); for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i])); } { - // arc lengths inside the original: sublinestring with some interpolated points + // arc lengths inside the original: sub-linestring with some interpolated points const auto sub = sub_linestring(ls, 1.5, 2.5); ASSERT_EQ(sub.size(), 3lu); EXPECT_NEAR(sub[0].x(), 1.5, eps); @@ -259,13 +259,13 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.max_path_arc_length = 0.0; // means no limit params.extra_arc_length = 1.0; params.expansion_method = "polygon"; - // 4m x 4m ego footprint + // 2m x 4m ego footprint params.ego_front_offset = 1.0; params.ego_rear_offset = -1.0; params.ego_left_offset = 2.0; params.ego_right_offset = -2.0; } - // we expect the expand the drivable area by 1m on each side + // we expect the drivable area to be expanded by 1m on each side drivable_area_expansion::expandDrivableArea( path, params, dynamic_objects, route_handler, path_lanes); // unchanged path points @@ -274,12 +274,15 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps); EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps); } + // expanded left bound - ASSERT_EQ(path.left_bound.size(), 2ul); + ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.left_bound[0].y, 2.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 2.0, eps); + EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); @@ -294,12 +297,12 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) { using drivable_area_expansion::calculateDistanceLimit; using drivable_area_expansion::linestring_t; - using drivable_area_expansion::multilinestring_t; + using drivable_area_expansion::multi_linestring_t; using drivable_area_expansion::polygon_t; { const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multilinestring_t uncrossable_lines = {}; + const multi_linestring_t uncrossable_lines = {}; const polygon_t expansion_polygon = { {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; const auto limit_distance = @@ -317,7 +320,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit) } { const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multilinestring_t uncrossable_lines = { + const multi_linestring_t uncrossable_lines = { {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; const polygon_t expansion_polygon = { {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}}; diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index 21538de073d35..3a809509641f4 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/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/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 47c619c310f4d..e1b74636f5ed1 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -13,7 +13,8 @@ // limitations under the License. #include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -35,7 +36,7 @@ using tier4_autoware_utils::Polygon2d; TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; vehicle_info_util::VehicleInfo vehicle_info; vehicle_info.max_longitudinal_offset_m = 4.0; @@ -128,7 +129,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { - using behavior_path_planner::utils::safety_check::createExtendedPolygon; + using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; @@ -176,19 +177,21 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { - using behavior_path_planner::utils::safety_check::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::RSSparams; { const double front_vel = 5.0; const double front_decel = -2.0; const double rear_vel = 10.0; const double rear_decel = -1.0; - BehaviorPathPlannerParameters params; + RSSparams params; params.rear_vehicle_reaction_time = 1.0; params.rear_vehicle_safety_time_margin = 1.0; params.longitudinal_distance_min_threshold = 3.0; + params.rear_vehicle_deceleration = rear_decel; + params.front_vehicle_deceleration = front_decel; - EXPECT_NEAR( - calcRssDistance(front_vel, rear_vel, front_decel, rear_decel, params), 63.75, epsilon); + EXPECT_NEAR(calcRssDistance(front_vel, rear_vel, params), 63.75, epsilon); } } diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 218989482ec97..f85c472c6d856 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -32,7 +32,7 @@ The manager launch crosswalk scene modules when the reference path conflicts cro The crosswalk module determines a stop position at least `stop_distance_from_object` away from the object.
- ![stop_distance_from_object](docs/stop_distance_from_object.svg){width=1000} + ![stop_distance_from_object](docs/stop_margin.svg){width=1000}
stop margin
@@ -44,14 +44,14 @@ The stop line is the reference point for the stopping position of the vehicle, b
- ![stop_distance_from_crosswalk](docs/stop_distance_from_crosswalk.svg){width=700} + ![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.
- ![far_object_threshold](docs/far_object_threshold.svg){width=1000} + ![far_object_threshold](docs/stop_line_margin.svg){width=1000}
stop at wide crosswalk
diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 6ff7104bfa992..4b0a1ffc4317b 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -4,8 +4,8 @@ common: show_processing_time: false # [-] whether to show processing time # param for input data - enable_rtc: true # 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 traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + enable_rtc: true # 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. # param for stop position stop_position: @@ -30,15 +30,24 @@ 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] # param for pass judge logic pass_judge: - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + 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_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 + 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. 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) 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: false + disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal timeout_no_intention_to_walk: 3.0 # [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. timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg index 7bd11ecf2ee39..04a208eee4166 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg @@ -55,12 +55,12 @@
- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin
- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin @@ -75,12 +75,12 @@
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin @@ -160,13 +160,13 @@
- ego_pass_later_margin + ego_pass_first_margin [s]
- ego_pass_later_margin [s] + ego_pass_first_margin [s] @@ -181,13 +181,13 @@
- ego_pass_pass_margin + ego_pass_later_margin [s]
- ego_pass_pass_margin [s] + ego_pass_later_margin [s] diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index c774c8876c850..992d1c48eb9f4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -66,17 +66,32 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.min_acc_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_acc"); + cp.max_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.max_jerk"); + cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic - cp.ego_pass_first_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_first_margin"); - cp.ego_pass_later_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); + cp.ego_pass_first_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_x"); + cp.ego_pass_first_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_y"); + cp.ego_pass_first_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); + cp.ego_pass_later_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_x"); + cp.ego_pass_later_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); + cp.ego_pass_later_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); + cp.max_offset_to_crosswalk_for_yield = + node.declare_parameter(ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); cp.stop_object_velocity = node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + cp.disable_yield_for_new_stopped_object = + node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); cp.timeout_no_intention_to_walk = node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 120f8ec8581be..0edfa6167e999 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,7 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; +using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; @@ -250,6 +252,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Set safe or unsafe setSafe(!nearest_stop_factor); + // Set distance + // NOTE: If no stop point is inserted, distance to the virtual stop line has to be calculated. + setDistanceToStop(*path, default_stop_pose, nearest_stop_factor); + // plan Go/Stop if (isActivated()) { planGo(*path, nearest_stop_factor); @@ -295,7 +301,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto & objects_ptr = planner_data_->predicted_objects; 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; @@ -307,45 +312,40 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range); // Update object state - updateObjectState(dist_ego_to_stop); + updateObjectState( + dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); + + // Check if ego moves forward enough to ignore yield. + if (!path_intersects.empty()) { + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double dist_ego2crosswalk = + calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); + if (dist_ego2crosswalk - base_link2front < planner_param_.max_offset_to_crosswalk_for_yield) { + return {}; + } + } // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop std::optional> nearest_stop_info; std::vector stop_factor_points; - const auto ignore_crosswalk = debug_data_.ignore_crosswalk = isRedSignalForPedestrians(); - for (const auto & object : objects_ptr->objects) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_uuid = toHexString(object.object_id); - - if (!isCrosswalkUserType(object)) { - continue; - } - - if (ignore_crosswalk) { - continue; - } - - // NOTE: Collision points are calculated on each predicted path - const auto collision_point = - getCollisionPoints(sparse_resample_path, object, attention_area, crosswalk_attention_range); - - for (const auto & cp : collision_point) { - const auto collision_state = - getCollisionState(obj_uuid, cp.time_to_collision, cp.time_to_vehicle); - debug_data_.collision_points.push_back(std::make_pair(cp, collision_state)); - + for (const auto & object : object_info_manager_.getObject()) { + const auto & collision_point = object.collision_point; + if (collision_point) { + const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } - stop_factor_points.push_back(obj_pos); + stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.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(cp.collision_point, dist_ego2cp - base_link2front); + nearest_stop_info = + std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); } } } @@ -406,7 +406,7 @@ std::pair CrosswalkModule::getAttentionRange( void CrosswalkModule::insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output) + PathWithLaneId & output) const { const auto stop_pose = planning_utils::insertDecelPoint(stop_point, output, target_velocity); if (!stop_pose) { @@ -558,9 +558,9 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } -std::vector CrosswalkModule::getCollisionPoints( - const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon2d & attention_area, - const std::pair & crosswalk_attention_range) +std::optional CrosswalkModule::getCollisionPoint( + const PathWithLaneId & ego_path, const PredictedObject & object, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { stop_watch_.tic(__func__); @@ -574,6 +574,8 @@ std::vector CrosswalkModule::getCollisionPoints( const auto obj_polygon = createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y); + double minimum_stop_dist = std::numeric_limits::max(); + std::optional nearest_collision_point{std::nullopt}; for (const auto & obj_path : object.kinematics.predicted_paths) { for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { const auto & p_obj_front = obj_path.path.at(i); @@ -586,26 +588,26 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - // Calculate nearest collision point - double minimum_stop_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point nearest_collision_point{}; + // Calculate nearest collision point among collisions with a predicted path + double local_minimum_stop_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point local_nearest_collision_point{}; for (const auto & p : tmp_intersection) { const auto cp = createPoint(p.x(), p.y(), ego_pos.z); const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp); - if (dist_ego2cp < minimum_stop_dist) { - minimum_stop_dist = dist_ego2cp; - nearest_collision_point = cp; + if (dist_ego2cp < local_minimum_stop_dist) { + local_minimum_stop_dist = dist_ego2cp; + local_nearest_collision_point = cp; } } const auto dist_ego2cp = - calcSignedArcLength(ego_path.points, ego_pos, nearest_collision_point); + calcSignedArcLength(ego_path.points, ego_pos, local_nearest_collision_point); constexpr double eps = 1e-3; const auto dist_obj2cp = calcArcLength(obj_path.path) < eps ? 0.0 - : calcSignedArcLength(obj_path.path, size_t(0), nearest_collision_point); + : calcSignedArcLength(obj_path.path, size_t(0), local_nearest_collision_point); if ( dist_ego2cp < crosswalk_attention_range.first || @@ -613,10 +615,13 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - ret.push_back( - createCollisionPoint(nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel)); - - debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + // Calculate nearest collision point among predicted paths + if (dist_ego2cp < minimum_stop_dist) { + minimum_stop_dist = dist_ego2cp; + nearest_collision_point = createCollisionPoint( + local_nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + } break; } @@ -626,7 +631,7 @@ std::vector CrosswalkModule::getCollisionPoints( logger_, planner_param_.show_processing_time, "%s : %f ms", __func__, stop_watch_.toc(__func__, true)); - return ret; + return nearest_collision_point; } CollisionPoint CrosswalkModule::createCollisionPoint( @@ -651,25 +656,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint( return collision_point; } -CollisionState CrosswalkModule::getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const -{ - // First, check if the object can be ignored - const auto obj_state = object_info_manager_.getState(obj_uuid); - if (obj_state == ObjectInfo::State::FULLY_STOPPED) { - return CollisionState::IGNORE; - } - - // Compare time to collision and vehicle - if (ttc + planner_param_.ego_pass_first_margin < ttv) { - return CollisionState::EGO_PASS_FIRST; - } - if (ttv + planner_param_.ego_pass_later_margin < ttc) { - return CollisionState::EGO_PASS_LATER; - } - return CollisionState::YIELD; -} - void CrosswalkModule::applySafetySlowDownSpeed( PathWithLaneId & output, const std::vector & path_intersects) { @@ -762,6 +748,8 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( const std::vector & path_intersects, const std::optional & stop_pose) const { + const auto & p = planner_param_; + if (path_intersects.size() < 2 || !stop_pose) { return {}; } @@ -772,26 +760,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( } const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; - if (planner_param_.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { + if (p.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { continue; } const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); - if (planner_param_.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { + 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 + planner_param_.stuck_vehicle_attention_range; + 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) { - return createStopFactor(*stop_pose, {obj_pos}); + // 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; + } + + 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 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 createStopFactor(*feasible_stop_pose, {obj_pos}); } } @@ -828,10 +838,16 @@ std::optional CrosswalkModule::getNearestStopFactor( return {}; } -void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) +void CrosswalkModule::updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { const auto & objects_ptr = planner_data_->predicted_objects; + const auto traffic_lights_reg_elems = + crosswalk_.regulatoryElementsAs(); + const bool has_traffic_light = !traffic_lights_reg_elems.empty(); + // Check if ego is yielding const bool is_ego_yielding = [&]() { const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold; @@ -840,13 +856,34 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) has_reached_stop_point; }(); + const auto ignore_crosswalk = isRedSignalForPedestrians(); + debug_data_.ignore_crosswalk = ignore_crosswalk; + // Update object state object_info_manager_.init(); for (const auto & object : objects_ptr->objects) { const auto obj_uuid = toHexString(object.object_id); + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; + + // calculate collision point and state + if (!isCrosswalkUserType(object)) { + continue; + } + if (ignore_crosswalk) { + continue; + } + + const auto collision_point = + getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( - obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, planner_param_); + obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, + has_traffic_light, collision_point, planner_param_); + + if (collision_point) { + const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); + debug_data_.collision_points.push_back(std::make_pair(*collision_point, collision_state)); + } } object_info_manager_.finalize(); } @@ -974,25 +1011,37 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( return polygon; } +void CrosswalkModule::setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor) +{ + // calculate stop position + const auto stop_pos = [&]() -> std::optional { + if (stop_factor) return stop_factor->stop_pose.position; + if (default_stop_pose) return default_stop_pose->position; + return std::nullopt; + }(); + + // Set distance + if (stop_pos) { + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); + setDistance(dist_ego2stop); + } +} + void CrosswalkModule::planGo( - PathWithLaneId & ego_path, const std::optional & stop_factor) + PathWithLaneId & ego_path, const std::optional & stop_factor) const { - if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); + if (!stop_factor.has_value()) { return; } - // Plan slow down const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path); insertDecelPointWithDebugInfo( stop_factor->stop_pose.position, std::max(planner_param_.min_slow_down_velocity, target_velocity), ego_path); - - // Set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } void CrosswalkModule::planStop( @@ -1006,7 +1055,7 @@ void CrosswalkModule::planStop( }(); if (!stop_factor) { - setDistance(std::numeric_limits::lowest()); + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 5000, "stop_factor is null"); return; } @@ -1016,11 +1065,5 @@ void CrosswalkModule::planStop( velocity_factor_.set( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); - - // set distance - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double dist_ego2stop = - calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); - setDistance(dist_ego2stop); } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 228be0d62ae29..961126bf4bca1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -56,6 +56,24 @@ using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; +namespace +{ +double interpolateEgoPassMargin( + const std::vector & x_vec, const std::vector & y_vec, const double target_x) +{ + for (size_t i = 0; i < x_vec.size(); ++i) { + if (target_x < x_vec.at(i)) { + if (i == 0) { + return y_vec.at(i); + } + const double ratio = (target_x - x_vec.at(i - 1)) / (x_vec.at(i) - x_vec.at(i - 1)); + return y_vec.at(i - 1) + ratio * (y_vec.at(i) - y_vec.at(i - 1)); + } + } + return y_vec.back(); +} +} // namespace + class CrosswalkModule : public SceneModuleInterface { public: @@ -76,12 +94,21 @@ class CrosswalkModule : public SceneModuleInterface double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; double stuck_vehicle_attention_range; + double min_acc_for_stuck_vehicle; + double max_jerk_for_stuck_vehicle; + double min_jerk_for_stuck_vehicle; // param for pass judge logic - double ego_pass_first_margin; - double ego_pass_later_margin; + std::vector ego_pass_first_margin_x; + std::vector ego_pass_first_margin_y; + double ego_pass_first_additional_margin; + std::vector ego_pass_later_margin_x; + std::vector ego_pass_later_margin_y; + double ego_pass_later_additional_margin; + double max_offset_to_crosswalk_for_yield; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; + bool disable_yield_for_new_stopped_object; double timeout_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data @@ -96,19 +123,22 @@ class CrosswalkModule : public SceneModuleInterface struct ObjectInfo { - // NOTE: FULLY_STOPPED means stopped object which can be ignored. - enum class State { STOPPED = 0, FULLY_STOPPED, OTHER }; - State state{State::OTHER}; + CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; - void updateState( - const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding, + geometry_msgs::msg::Point position{}; + std::optional collision_point{}; + + void transitState( + const rclcpp::Time & now, const double vel, const bool is_ego_yielding, + const bool has_traffic_light, const std::optional & collision_point, const PlannerParam & planner_param) { - const bool is_stopped = obj_vel < planner_param.stop_object_velocity; + const bool is_stopped = vel < planner_param.stop_object_velocity; + // Check if the object can be ignored if (is_stopped) { - if (state == State::FULLY_STOPPED) { + if (collision_state == CollisionState::IGNORE) { return; } @@ -117,15 +147,51 @@ class CrosswalkModule : public SceneModuleInterface } const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; - if ((is_ego_yielding || planner_param.disable_stop_for_yield_cancel) && !intent_to_cross) { - state = State::FULLY_STOPPED; - } else { - // NOTE: Object may start moving - state = State::STOPPED; + if ( + (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && + !intent_to_cross) { + collision_state = CollisionState::IGNORE; + return; } } else { time_to_start_stopped = std::nullopt; - state = State::OTHER; + } + + // Compare time to collision and vehicle + if (collision_point) { + // Check if ego will pass first + const double ego_pass_first_additional_margin = + collision_state == CollisionState::EGO_PASS_FIRST + ? 0.0 + : planner_param.ego_pass_first_additional_margin; + const double ego_pass_first_margin = interpolateEgoPassMargin( + planner_param.ego_pass_first_margin_x, planner_param.ego_pass_first_margin_y, + collision_point->time_to_collision); + if ( + collision_point->time_to_collision + ego_pass_first_margin + + ego_pass_first_additional_margin < + collision_point->time_to_vehicle) { + collision_state = CollisionState::EGO_PASS_FIRST; + return; + } + + // Check if ego will pass later + const double ego_pass_later_additional_margin = + collision_state == CollisionState::EGO_PASS_LATER + ? 0.0 + : planner_param.ego_pass_later_additional_margin; + const double ego_pass_later_margin = interpolateEgoPassMargin( + planner_param.ego_pass_later_margin_x, planner_param.ego_pass_later_margin_y, + collision_point->time_to_vehicle); + if ( + collision_point->time_to_vehicle + ego_pass_later_margin + + ego_pass_later_additional_margin < + collision_point->time_to_collision) { + collision_state = CollisionState::EGO_PASS_LATER; + return; + } + collision_state = CollisionState::YIELD; + return; } } }; @@ -133,19 +199,29 @@ class CrosswalkModule : public SceneModuleInterface { void init() { current_uuids_.clear(); } void update( - const std::string & uuid, const double obj_vel, const rclcpp::Time & now, - const bool is_ego_yielding, const PlannerParam & planner_param) + 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) { // update current uuids current_uuids_.push_back(uuid); // add new object if (objects.count(uuid) == 0) { - objects.emplace(uuid, ObjectInfo{}); + if ( + has_traffic_light && planner_param.disable_stop_for_yield_cancel && + planner_param.disable_yield_for_new_stopped_object) { + objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); + } else { + objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); + } } // update object state - objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, planner_param); + objects.at(uuid).transitState( + now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + objects.at(uuid).collision_point = collision_point; + objects.at(uuid).position = position; } void finalize() { @@ -162,7 +238,19 @@ class CrosswalkModule : public SceneModuleInterface objects.erase(obsolete_uuid); } } - ObjectInfo::State getState(const std::string & uuid) const { return objects.at(uuid).state; } + + std::vector getObject() const + { + std::vector object_info_vec; + for (auto object : objects) { + object_info_vec.push_back(object.second); + } + return object_info_vec; + } + CollisionState getCollisionState(const std::string & uuid) const + { + return objects.at(uuid).collision_state; + } std::unordered_map objects; std::vector current_uuids_; @@ -198,16 +286,21 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & stop_pose) const; - std::vector getCollisionPoints( + std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, - const Polygon2d & attention_area, const std::pair & crosswalk_attention_range); + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); std::optional getNearestStopFactor( const PathWithLaneId & ego_path, const std::optional & stop_factor_for_crosswalk_users, const std::optional & stop_factor_for_stuck_vehicles); - void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor); + void setDistanceToStop( + const PathWithLaneId & ego_path, + const std::optional & default_stop_pose, + const std::optional & stop_factor); + + void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor) const; void planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, @@ -220,7 +313,7 @@ class CrosswalkModule : public SceneModuleInterface void insertDecelPointWithDebugInfo( const geometry_msgs::msg::Point & stop_point, const float target_velocity, - PathWithLaneId & output); + PathWithLaneId & output) const; std::pair clampAttentionRangeByNeighborCrosswalks( const PathWithLaneId & ego_path, const double near_attention_range, @@ -231,9 +324,6 @@ class CrosswalkModule : public SceneModuleInterface const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, const geometry_msgs::msg::Vector3 & obj_vel) const; - CollisionState getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const; - float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; @@ -245,7 +335,9 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects) const; - void updateObjectState(const double dist_ego_to_stop); + void updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); bool isRedSignalForPedestrians() const; diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 3e6d5bc81ff92..8ff78dac06716 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED) autoware_package() 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 @@ -13,4 +15,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp ) +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} +) + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3ab642abe1e8f..3e62ab75d23cd 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -8,7 +8,7 @@ The _intersection_ module is responsible for safely going through urban intersec 2. recognizing the occluded area in the intersection 3. reacting to arrow signals of associated traffic lights -The module is desinged to be agnositc to left-hand/right-hand traffic rules and works on crossroads, T-shape junctions, etc. +The module is designed to be agnostic to left-hand/right-hand traffic rules and works on crossroads, T-shape junctions, etc. ![topology](./docs/intersection-topology.drawio.svg) @@ -26,7 +26,7 @@ This module is activated when the path contains the lanes with `turn_direction` The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority. -`Intersection Area`, which is supposed to be defined on the HDMap, is an area convering the entire intersection. +`Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection. ![attention_area](./docs/intersection-attention.drawio.svg) @@ -38,7 +38,7 @@ Following table shows an example of how to assign `right_of_way` tag and set `yi | ------------------------------ | --------------------------------------------------------------- | ------------------------------------------------ | | straight | Highest priority of all | Priority over left/right lanes of the same group | | left(Left hand traffic) | Priority over the other group and right lanes of the same group | Priority over right lanes of the same group | -| right(Left hand traffic) | Priority only over the other group | priority only over the other gruop | +| right(Left hand traffic) | Priority only over the other group | priority only over the other group | | left(Right hand traffic) | Priority only over the other group | Priority only over the other group | | right(Right hand traffic) | Priority over the other group and left lanes of the same group | priority over left lanes of the same group | @@ -108,7 +108,7 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | | `stuck_vehicle.stuck_vehicle_ignore_dist` | double | [m] length behind the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threhsold for stuck vehicle detection | +| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | | `collision_detection.collision_start_margin_time` | double | [s] time margin for the beginning of collision with upcoming vehicle | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index efe0efa3d7310..2b3ca11bbe750 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -46,11 +46,13 @@ min_vehicle_brake_for_rss: -2.5 # [m/s^2] max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph denoise_kernel: 1.0 # [m] - pub_debug_grid: false + possible_object_bbox: [1.0, 2.0] # [m x m] + ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h enable_rtc: # 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 intersection: true intersection_to_occlusion: true merge_from_private: + stop_line_margin: 3.0 stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 25df9f2385d74..1d6067d4be322 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -20,14 +20,9 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs behavior_velocity_planner_common - cv_bridge geometry_msgs - grid_map_cv - grid_map_ros interpolation lanelet2_extension - libboost-dev - libopencv-dev magic_enum motion_utils nav_msgs diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index dc5b2b8373f04..ae8eee39d6556 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -221,6 +221,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + for (size_t j = 0; j < debug_data_.occlusion_polygons.size(); ++j) { + const auto & p = debug_data_.occlusion_polygons.at(j); + appendMarkerArray( + debug::createPolygonMarkerArray( + p, "occlusion_polygons", lane_id_ + j, now, 0.3, 0.0, 0.0, 1.0, 0.0, 0.0), + &debug_marker_array, now); + } + return debug_marker_array; } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 8064ccbd33430..6a2b7200abb2e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -113,7 +113,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.occlusion.max_vehicle_velocity_for_rss = node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); - ip.occlusion.pub_debug_grid = node.declare_parameter(ns + ".occlusion.pub_debug_grid"); + ip.occlusion.possible_object_bbox = + node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = + node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); } void IntersectionModuleManager::launchNewModules( @@ -263,7 +266,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); mp.attention_area_length = node.get_parameter("intersection.common.attention_area_length").as_double(); - mp.stop_line_margin = node.get_parameter("intersection.common.stop_line_margin").as_double(); + mp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 086e844cd5e81..7ae549ce8b274 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -12,26 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include -#include - -#if __has_include() -#include -#else -#include -#endif -// #include -// #include #include "scene_intersection.hpp" + #include "util.hpp" #include #include #include +#include +#include +#include +#include #include #include @@ -93,12 +84,10 @@ IntersectionModule::IntersectionModule( planner_param_.collision_detection.state_transit_margin_time); before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); before_creep_state_machine_.setState(StateMachine::State::STOP); - if (enable_occlusion_detection) { - occlusion_grid_pub_ = node_.create_publisher( - "~/debug/intersection/occlusion_grid", rclcpp::QoS(1).transient_local()); - } stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); stuck_private_area_timeout_.setState(StateMachine::State::STOP); + decision_state_pub_ = + node_.create_publisher("~/debug/intersection/decision_state", 1); } void IntersectionModule::initializeRTCStatus() @@ -661,6 +650,35 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + std::string decision_type = "intersection" + std::to_string(module_id_) + " : "; + if (std::get_if(&decision_result)) { + decision_type += "Indecisive"; + } + if (std::get_if(&decision_result)) { + decision_type += "StuckStop"; + } + if (std::get_if(&decision_result)) { + decision_type += "NonOccludedCollisionStop"; + } + if (std::get_if(&decision_result)) { + decision_type += "FirstWaitBeforeOcclusion"; + } + if (std::get_if(&decision_result)) { + decision_type += "PeekingTowardOcclusion"; + } + if (std::get_if(&decision_result)) { + decision_type += "OccludedCollisionStop"; + } + if (std::get_if(&decision_result)) { + decision_type += "Safe"; + } + if (std::get_if(&decision_result)) { + decision_type += "TrafficLightArrowSolidOn"; + } + 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; @@ -690,18 +708,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { RCLCPP_DEBUG(logger_, "splineInterpolate failed"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } 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_) { - const auto lanelets_on_path = - planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); intersection_lanelets_ = util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, planner_param_.common.attention_area_length, @@ -715,6 +735,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); if (conflicting_lanelets.empty() || !first_conflicting_area) { + is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -727,6 +748,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.common.stop_line_margin, planner_param_.occlusion.peeking_offset, path); if (!intersection_stop_lines_opt) { RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); @@ -734,8 +756,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, pass_judge_line_idx] = intersection_stop_lines; - const auto ego_lane_with_next_lane = util::getEgoLaneWithNextLane( - *path, associative_ids_, planner_data_->vehicle_info_.vehicle_width_m); + const auto path_lanelets_opt = util::generatePathLanelets( + lanelets_on_path, *path, associative_ids_, closest_idx, + planner_data_->vehicle_info_.vehicle_width_m); + if (!path_lanelets_opt.has_value()) { + RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); + return IntersectionModule::Indecisive{}; + } + const auto path_lanelets = path_lanelets_opt.value(); + + const auto ego_lane_with_next_lane = + path_lanelets.next.has_value() + ? std::vector< + lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} + : std::vector{path_lanelets.ego_or_entry2exit}; const bool stuck_detected = checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); @@ -753,6 +787,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool timeout = (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); if (!timeout) { + is_peeking_ = false; return IntersectionModule::StuckStop{ stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines}; } @@ -760,11 +795,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!first_attention_area) { RCLCPP_DEBUG(logger_, "attention area is empty"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } if (default_stop_line_idx == 0) { RCLCPP_DEBUG(logger_, "stop line index is 0"); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -775,10 +812,16 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); const bool is_over_default_stop_line = util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); - const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); + 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_vel_thr); // if ego is over the pass judge line and not stopped - if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + if (is_peeking_) { + // do nothing + RCLCPP_DEBUG(logger_, "peeking now"); + } else if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { RCLCPP_DEBUG( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing @@ -788,6 +831,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // activated_: current RTC approval is_permanent_go_ = true; RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); + is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -812,9 +856,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); + const auto target_objects = + filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + const bool has_collision = checkCollision( - *path, attention_lanelets, adjacent_lanelets, intersection_area, ego_lane_with_next_lane, - closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -822,6 +868,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (tl_arrow_solid_on) { + is_peeking_ = false; return TrafficLightArrowSolidOn{has_collision, intersection_stop_lines}; } @@ -835,12 +882,21 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); + std::vector parked_attention_objects; + std::copy_if( + target_objects.objects.begin(), target_objects.objects.end(), + std::back_inserter(parked_attention_objects), + [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { + return std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; + }); const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), occlusion_dist_thr) + occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) : true; // check safety @@ -852,25 +908,28 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool approached_stop_line = (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); const bool over_stop_line = (dist_stopline < 0.0); - const bool is_stopped = planner_data_->isVehicleStopped(); + const bool is_stopped = + planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); if (over_stop_line) { before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { if (has_collision) { + is_peeking_ = true; return IntersectionModule::OccludedCollisionStop{ default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; } else { + is_peeking_ = true; return IntersectionModule::PeekingTowardOcclusion{ occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; } } else { if (is_stopped && approached_stop_line) { // start waiting at the first stop line - before_creep_state_machine_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_); + before_creep_state_machine_.setState(StateMachine::State::GO); } + is_peeking_ = true; return IntersectionModule::FirstWaitBeforeOcclusion{ default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; @@ -879,9 +938,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_over_default_stopLine = util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); const auto stop_line_idx = is_over_default_stopLine ? closest_idx : default_stop_line_idx; + is_peeking_ = false; return IntersectionModule::NonOccludedCollisionStop{stop_line_idx, intersection_stop_lines}; } + is_peeking_ = false; return IntersectionModule::Safe{intersection_stop_lines}; } @@ -922,13 +983,10 @@ bool IntersectionModule::checkStuckVehicle( return is_stuck; } -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on) + const std::optional & intersection_area) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -975,6 +1033,17 @@ bool IntersectionModule::checkCollision( target_objects.objects.push_back(object); } } + return target_objects; +} + +bool IntersectionModule::checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const bool tl_arrow_solid_on) +{ + 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 @@ -983,14 +1052,13 @@ bool IntersectionModule::checkCollision( planner_param_.common.intersection_velocity, planner_param_.collision_detection.minimum_ego_predicted_velocity); const double passing_time = time_distance_array.back().first; + auto target_objects = objects; util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const auto & ego_lane = ego_lane_with_next_lane.front(); - const double distance_until_intersection = - util::calcDistanceUntilIntersectionLanelet(ego_lane, path, closest_idx); - const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area @@ -1002,7 +1070,6 @@ bool IntersectionModule::checkCollision( : planner_param_.collision_detection.normal.collision_end_margin_time; bool collision_detected = false; for (const auto & object : target_objects.objects) { - bool has_collision = false; for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1011,93 +1078,82 @@ bool IntersectionModule::checkCollision( continue; } - std::vector predicted_poses; - for (const auto & pose : predicted_path.path) { - predicted_poses.push_back(pose); - } - has_collision = bg::intersects(ego_poly, to_bg2d(predicted_poses)); - if (has_collision) { - const auto first_itr = std::adjacent_find( - predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); - }); - const auto last_itr = std::adjacent_find( - predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); - }); - 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_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()) { - 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( + // collision point + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), + [&ego_poly](const auto & a, const auto & b) { + return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + }); + 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](const auto & a, const auto & b) { + return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + }); + 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_exit_time + collision_end_margin_time, + ref_object_enter_time - collision_start_margin_time, [](const auto & a, const double b) { return a.first < b; }); - if (end_time_distance_itr == time_distance_array.end()) { - 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 - - distance_until_intersection); - const double end_arc_length = std::max( - 0.0, closest_arc_coords.length + (*end_time_distance_itr).second + base_link2front - - distance_until_intersection); - - long double lanes_length = 0.0; - std::vector ego_lane_with_next_lanes; - - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_odometry->pose); - for (const auto & lane : lanelets_on_path) { - lanes_length += bg::length(lane.centerline()); - ego_lane_with_next_lanes.push_back(lane); - if (lanes_length > start_arc_length && lanes_length < end_arc_length) { - break; - } - } - const auto trimmed_ego_polygon = - getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length); - - Polygon2d polygon{}; - for (const auto & p : trimmed_ego_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); + 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; + } - polygon.outer().emplace_back(polygon.outer().front()); - - bg::correct(polygon); - - debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + 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); - debug_data_.candidate_collision_object_polygons.emplace_back( - toGeomPoly(footprint_polygon)); - if (bg::intersects(polygon, footprint_polygon)) { - collision_detected = true; - break; - } - } - if (collision_detected) { - debug_data_.conflicting_targets.objects.push_back(object); + 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; } } + if (collision_detected) { + debug_data_.conflicting_targets.objects.push_back(object); + break; + } } } @@ -1110,7 +1166,9 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, const double occlusion_dist_thr) + const std::vector & lane_divisions, + const std::vector & parked_attention_objects, + const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); @@ -1138,6 +1196,8 @@ bool IntersectionModule::isOcclusionCleared( cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); @@ -1201,8 +1261,8 @@ bool IntersectionModule::isOcclusionCleared( for (const auto & common_area : common_areas) { std::vector adjacent_lane_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); + const int idx_x = std::floor((p.x() - origin.x) / resolution); + const int idx_y = std::floor((p.y() - origin.y) / resolution); adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); } adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon); @@ -1231,7 +1291,7 @@ bool IntersectionModule::isOcclusionCleared( cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw); // (3.1) apply morphologyEx cv::Mat occlusion_mask; - const int morph_size = std::ceil(planner_param_.occlusion.denoise_kernel / resolution); + const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); cv::morphologyEx( occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); @@ -1243,7 +1303,6 @@ bool IntersectionModule::isOcclusionCleared( const double distance_min = -distance_max; const int undef_pixel = 255; const int max_cost_pixel = 254; - auto dist2pixel = [=](const double dist) { return std::min( max_cost_pixel, @@ -1252,8 +1311,8 @@ bool IntersectionModule::isOcclusionCleared( auto pixel2dist = [=](const int pixel) { return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; }; - const int zero_dist_pixel = dist2pixel(0.0); + const int parked_vehicle_pixel = zero_dist_pixel - 1; // magic auto coord2index = [&](const double x, const double y) { const int idx_x = (x - origin.x) / resolution; @@ -1266,6 +1325,7 @@ bool IntersectionModule::isOcclusionCleared( cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); + // (4.1) fill zero_dist_pixel on the path const auto [lane_start, lane_end] = lane_attention_interval_ip; for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { const auto & path_pos = path_ip.points.at(i).point.pose.position; @@ -1277,6 +1337,31 @@ bool IntersectionModule::isOcclusionCleared( projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; } + // (4.2) fill parked_vehicle_pixel to parked_vehicles (both positive and negative) + for (const auto & parked_attention_object : parked_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(parked_attention_object); + std::vector common_areas; + bg::intersection(obj_poly, grid_poly, common_areas); + if (common_areas.empty()) continue; + 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]); + } + std::vector> parked_attention_object_area_cv_polygons; + for (const auto & common_area : common_areas) { + std::vector parked_attention_object_area_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); + parked_attention_object_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + parked_attention_object_area_cv_polygons.push_back(parked_attention_object_area_cv_polygon); + } + for (const auto & poly : parked_attention_object_area_cv_polygons) { + cv::fillPoly(distance_grid, poly, cv::Scalar(parked_vehicle_pixel), cv::LINE_AA); + } + } + for (const auto & lane_division : lane_divisions) { const auto & divisions = lane_division.divisions; for (const auto & division : divisions) { @@ -1285,8 +1370,9 @@ bool IntersectionModule::isOcclusionCleared( int projection_ind = -1; std::optional> cost_prev_checkpoint = std::nullopt; // cost, x, y, projection_ind - for (const auto & point : division) { - const auto [valid, idx_x, idx_y] = coord2index(point.x(), point.y()); + for (auto point = division.begin(); point != division.end(); point++) { + const double x = point->x(), y = point->y(); + const auto [valid, idx_x, idx_y] = coord2index(x, y); // exited grid just now if (is_in_grid && !valid) break; @@ -1309,16 +1395,27 @@ bool IntersectionModule::isOcclusionCleared( zero_dist_cell_found = true; projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); assert(projection_ind >= 0); - cost_prev_checkpoint = std::make_optional>( - 0.0, point.x(), point.y(), projection_ind); + cost_prev_checkpoint = + std::make_optional>(0.0, x, y, projection_ind); continue; } + // hit positive parked vehicle + if (zero_dist_cell_found && pixel == parked_vehicle_pixel) { + while (point != division.end()) { + const double x = point->x(), y = point->y(); + const auto [valid, idx_x, idx_y] = coord2index(x, y); + if (valid) occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + point++; + } + break; + } + if (zero_dist_cell_found) { // finally traversed to defined cell (first half) const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = cost_prev_checkpoint.value(); - const double dy = point.y() - prev_checkpoint_y, dx = point.x() - prev_checkpoint_x; + const double dy = y - prev_checkpoint_y, dx = x - prev_checkpoint_x; double new_dist = prev_cost + std::hypot(dy, dx); const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); const double cur_dist = pixel2dist(pixel); @@ -1331,65 +1428,83 @@ bool IntersectionModule::isOcclusionCleared( projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); cost_prev_checkpoint = std::make_optional>( - new_dist, point.x(), point.y(), projection_ind); + new_dist, x, y, projection_ind); } } } } - // clean-up and find nearest risk + 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) { + std::vector valid_contour; + for (const auto & p : contour) { + if (distance_grid.at(p.y, p.x) == undef_pixel) { + continue; + } + valid_contour.push_back(p); + } + if (valid_contour.size() <= 2) { + continue; + } + std::vector approx_contour; + cv::approxPolyDP( + valid_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); + } + const int min_cost_thr = dist2pixel(occlusion_dist_thr); int min_cost = undef_pixel - 1; - int max_cost = 0; - std::optional min_cost_projection_ind = std::nullopt; geometry_msgs::msg::Point nearest_occlusion_point; - for (int i = 0; i < width; ++i) { - for (int j = 0; j < height; ++j) { - const int pixel = static_cast(distance_grid.at(height - 1 - j, i)); - const bool occluded = (occlusion_mask.at(height - 1 - j, i) == 255); + for (const auto & occlusion_contour : valid_contours) { + for (const auto & p : occlusion_contour) { + const int pixel = static_cast(distance_grid.at(p.y, p.x)); + const bool occluded = (occlusion_mask.at(p.y, p.x) == 255); if (pixel == undef_pixel || !occluded) { - // ignore outside of cropped - // some cell maybe undef still - distance_grid.at(height - 1 - j, i) = 0; continue; } - if (max_cost < pixel) { - max_cost = pixel; - } - const int projection_ind = projection_ind_grid.at(height - 1 - j, i); if (pixel < min_cost) { - assert(projection_ind >= 0); min_cost = pixel; - min_cost_projection_ind = projection_ind; - nearest_occlusion_point.x = origin.x + i * resolution; - nearest_occlusion_point.y = origin.y + j * resolution; - nearest_occlusion_point.z = origin.z + distance_max * pixel / max_cost_pixel; + nearest_occlusion_point.x = origin.x + p.x * resolution; + nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution; + nearest_occlusion_point.z = origin.z; } } } - debug_data_.nearest_occlusion_point = nearest_occlusion_point; - - if (planner_param_.occlusion.pub_debug_grid) { - cv::Mat distance_grid_heatmap; - cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET); - grid_map::GridMap occlusion_grid({"elevation"}); - occlusion_grid.setFrameId("map"); - occlusion_grid.setGeometry( - grid_map::Length(width * resolution, height * resolution), resolution, - grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2)); - cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE); - cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE); - grid_map::GridMapCvConverter::addLayerFromImage( - distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */, - origin.z + distance_max /* elevation for 255 */); - grid_map::GridMapCvConverter::addColorLayerFromImage( - distance_grid_heatmap, "color", occlusion_grid); - occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid)); - } - if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) { + if (min_cost > min_cost_thr) { return true; } else { + debug_data_.nearest_occlusion_point = nearest_occlusion_point; return false; } } @@ -1418,9 +1533,8 @@ bool IntersectionModule::checkFrontVehicleDeceleration( // get the nearest centerpoint to object std::vector dist_obj_center_points; for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, p)); - const int obj_closest_centerpoint_idx = std::distance( - dist_obj_center_points.begin(), + dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, +p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); // find two center_points whose distances from `closest_centerpoint` cross stopping_distance double acc_dist_prev = 0.0, acc_dist = 0.0; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 893761e834f3a..774b68710be7f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -20,13 +20,12 @@ #include #include #include -#include #include #include #include #include -#include +#include #include #include @@ -108,7 +107,8 @@ class IntersectionModule : public SceneModuleInterface double min_vehicle_brake_for_rss; double max_vehicle_velocity_for_rss; double denoise_kernel; - bool pub_debug_grid; + std::vector possible_object_bbox; + double ignore_parked_vehicle_speed_threshold; } occlusion; }; @@ -210,6 +210,7 @@ class IntersectionModule : public SceneModuleInterface std::string turn_direction_; bool is_go_out_ = false; bool is_permanent_go_ = false; + bool is_peeking_ = false; // Parameter PlannerParam planner_param_; std::optional intersection_lanelets_; @@ -218,7 +219,7 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; + std::optional> occlusion_attention_divisions_; // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop @@ -249,13 +250,16 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const util::IntersectionStopLines & intersection_stop_lines); - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on); + const std::optional & intersection_area) const; + + bool checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, + const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const bool tl_arrow_solid_on); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, @@ -263,7 +267,10 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, const double occlusion_dist_thr); + const std::vector & lane_divisions, + const std::vector & + parked_attention_objects, + const double occlusion_dist_thr); /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -274,7 +281,7 @@ class IntersectionModule : public SceneModuleInterface */ util::DebugData debug_data_; - rclcpp::Publisher::SharedPtr occlusion_grid_pub_; + rclcpp::Publisher::SharedPtr decision_state_pub_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 13d8b10b8f76c..a987ce9af9dfe 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -315,6 +315,10 @@ std::optional generateIntersectionStopLines( intersection_stop_lines.default_stop_line) { intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines.default_stop_line; } + if ( + intersection_stop_lines.occlusion_peeking_stop_line > intersection_stop_lines.pass_judge_line) { + intersection_stop_lines.pass_judge_line = intersection_stop_lines.occlusion_peeking_stop_line; + } return intersection_stop_lines; } @@ -705,7 +709,7 @@ bool isTrafficLightArrowActivated( return false; } -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) @@ -774,15 +778,15 @@ std::vector generateDetectionLaneDivisions( auto & branch = branches[(ind2id[src])]; int node_iter = ind2id[src]; while (true) { - const auto & dsts = adjacency[(id2ind[node_iter])]; + const auto & destinations = adjacency[(id2ind[node_iter])]; // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(dsts.begin(), dsts.end(), true); - if (next == dsts.end()) { + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { branch.push_back(node_iter); break; } branch.push_back(node_iter); - node_iter = ind2id[std::distance(dsts.begin(), next)]; + node_iter = ind2id[std::distance(destinations.begin(), next)]; } } for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { @@ -814,9 +818,9 @@ std::vector generateDetectionLaneDivisions( } // (3) discretize each merged lanelet - std::vector detection_divisions; + std::vector detection_divisions; for (const auto & [last_lane_id, branch] : merged_branches) { - DescritizedLane detection_division; + DiscretizedLane detection_division; detection_division.lane_id = last_lane_id; const auto detection_lanelet = branch.first; const double area = branch.second; @@ -868,29 +872,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -lanelet::ConstLanelets getEgoLaneWithNextLane( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const double width) -{ - // NOTE: findLaneIdsInterval returns (start, end) of associative_ids - const auto ego_lane_interval_opt = findLaneIdsInterval(path, associative_ids); - if (!ego_lane_interval_opt) { - return lanelet::ConstLanelets({}); - } - const auto [ego_start, ego_end] = ego_lane_interval_opt.value(); - if (ego_end < path.points.size() - 1) { - const int next_id = path.points.at(ego_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(); - return { - planning_utils::generatePathLanelet(path, ego_start, next_start + 1, width), - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width)}; - } - } - return {planning_utils::generatePathLanelet(path, ego_start, ego_end, width)}; -} - static bool isTargetStuckVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -919,8 +900,10 @@ bool checkStuckVehicleInIntersection( if (!isTargetStuckVehicleType(object)) { continue; // not target vehicle type } - const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (obj_v > stuck_vehicle_vel_thr) { + 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 } @@ -1132,5 +1115,61 @@ void IntersectionLanelets::update( } } +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; +} + +std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::set & associative_ids, const size_t closest_idx, const double width) +{ + const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + + 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_opt.value(); + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + planning_utils::generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width); + 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 = + planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + return path_lanelets; +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 081ade30f8f7d..287a75979927b 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -114,7 +114,7 @@ bool isTrafficLightArrowActivated( lanelet::ConstLanelet lane, const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); @@ -126,10 +126,6 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -lanelet::ConstLanelets getEgoLaneWithNextLane( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const double width); - bool checkStuckVehicleInIntersection( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, @@ -160,6 +156,11 @@ double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); +std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::set & associative_ids, const size_t closest_idx, const double width); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4a87b993dbe73..26187d75ff53c 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -49,6 +49,7 @@ struct DebugData autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; std::optional nearest_occlusion_point = std::nullopt; std::optional nearest_occlusion_projection_point = std::nullopt; + std::vector occlusion_polygons; }; struct InterpolatedPathInfo @@ -114,7 +115,7 @@ struct IntersectionLanelets std::optional first_attention_area_ = std::nullopt; }; -struct DescritizedLane +struct DiscretizedLane { int lane_id; // discrete fine lines from left to right @@ -131,6 +132,18 @@ struct IntersectionStopLines size_t pass_judge_line; }; +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; +}; + } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg b/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg index 8acecba2f558d..a5601e4e0c503 100644 --- a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg +++ b/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg @@ -304,7 +304,7 @@
- colliision + collision
diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg b/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg index 55693e2a2688c..650194484e933 100644 --- a/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg +++ b/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg @@ -220,14 +220,14 @@
- ---  safe velocity to stop before collisopn + ---  safe velocity to stop before collision
- ---- safe slow donw velocity + ---- safe slow down velocity
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 0a172c7f5544b..21a8cce93a1be 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 @@ -186,15 +186,19 @@ bool isVehicle(const PredictedObject & obj) bool isStuckVehicle(const PredictedObject & obj, const double min_vel) { if (!isVehicle(obj)) return false; - const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::abs(obj_vel) <= min_vel; + const auto & obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + return obj_vel_norm <= min_vel; } bool isMovingVehicle(const PredictedObject & obj, const double min_vel) { if (!isVehicle(obj)) return false; - const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::abs(obj_vel) > min_vel; + const auto & obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + return obj_vel_norm > min_vel; } std::vector extractVehicles( diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index a3089cbc1fde3..da46055460a99 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -101,7 +101,7 @@ at its current velocity or at half the velocity of the path points, whichever is ###### Dynamic objects Two methods are used to estimate the time when a dynamic objects with reach some point. -If `objects.use_predicted_paths` is set to `true`, the predicted path of the dynamic object is used. +If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. #### 5. Path update @@ -138,10 +138,11 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | -------------- | ------ | ------------------------------------------------------------------------------------------------------ | | `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | -| Parameter /objects | Type | Description | -| --------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | +| Parameter /objects | Type | Description | +| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | +| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | | Parameter /overlap | Type | Description | | ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index c501599b4ae1c..dd4c1c610261c 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -16,6 +16,7 @@ minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored use_predicted_paths: true # if true, use the predicted paths to estimate future positions. # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. + predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index a164a03a9aa43..163c8632e40c7 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -40,16 +40,38 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx) return dist / v; } +bool object_is_incoming( + const lanelet::BasicPoint2d & object_position, + const std::shared_ptr route_handler, + const lanelet::ConstLanelet & lane) +{ + const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); + if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; + for (const auto & lls : lanelets) + for (const auto & ll : lls) + if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; + return false; +} + std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double min_confidence, const rclcpp::Logger & logger) { + // skip the dynamic object if it is not in a lane preceding the overlapped lane + // lane changes are intentionally not considered + const auto object_point = lanelet::BasicPoint2d( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; + const auto max_deviation = object.shape.dimensions.y * 2.0; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { + if (predicted_path.confidence < min_confidence) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); @@ -243,7 +265,7 @@ bool ttc_condition( return collision_during_overlap || ttc_is_bellow_threshold; } -bool object_is_incoming( +bool will_collide_on_range( const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) { RCLCPP_DEBUG( @@ -274,9 +296,11 @@ bool should_not_enter( continue; // skip objects with velocity bellow a threshold } // skip objects that are already on the interval - const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range(object, range, logger) - : object_time_to_range(object, range, inputs, logger); + const auto enter_exit_time = + params.objects_use_predicted_paths + ? object_time_to_range( + object, range, inputs.route_handler, params.objects_min_confidence, logger) + : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); continue; // skip objects that are not driving towards the overlapping range @@ -284,7 +308,7 @@ bool should_not_enter( range_times.object.enter_time = enter_exit_time->first; range_times.object.exit_time = enter_exit_time->second; - if (object_is_incoming(range_times, params, logger)) return true; + if (will_collide_on_range(range_times, params, logger)) return true; } return false; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 6af03bc53ea3c..bf6bf81e506cf 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -56,28 +56,33 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// but may not exist (e.g,, predicted path ends before reaching the end of the range) /// @param [in] object dynamic object /// @param [in] range overlapping range +/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] min_confidence minimum confidence to consider a predicted path +/// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range); + const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const std::shared_ptr route_handler, const double min_confidence, + const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range /// @param [in] object dynamic object /// @param [in] range overlapping range -/// @param [in] lanelets objects to consider -/// @param [in] route_handler route handler used to estimate the path of the dynamic object +/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route +/// handler, lanelets) +/// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const lanelet::ConstLanelets & lanelets, - const std::shared_ptr & route_handler); + const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) /// @param [in] range_times times when ego and the object enter/exit the range /// @param [in] params parameters /// @param [in] logger ros logger -bool object_is_incoming( +bool will_collide_on_range( const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); /// @brief check whether we should avoid entering the given range /// @param [in] range the range to check diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 1eccd8773cc92..22fb29110466a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -44,6 +44,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.objects_min_vel = node.declare_parameter(ns + ".objects.minimum_velocity"); pp.objects_use_predicted_paths = node.declare_parameter(ns + ".objects.use_predicted_paths"); + pp.objects_min_confidence = + node.declare_parameter(ns + ".objects.predicted_path_min_confidence"); pp.overlap_min_dist = node.declare_parameter(ns + ".overlap.minimum_distance"); pp.overlap_extra_length = node.declare_parameter(ns + ".overlap.extra_length"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index e4ba8fbfead81..85266d779f34e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -45,6 +45,7 @@ struct PlannerParam bool objects_use_predicted_paths; // # whether to use the objects' predicted paths double objects_min_vel; // # [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // # minimum confidence to consider a predicted path double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap diff --git a/planning/behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_stop_line_module/README.md index 5be11c00e814d..f373afc2351bf 100644 --- a/planning/behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_stop_line_module/README.md @@ -12,18 +12,20 @@ This module is activated when there is a stop line in a target lane. ### Module Parameters -| Parameter | Type | Description | -| --------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | -| `stop_check_dist` | double | when the vehicle is within `stop_check_dist` from stop_line and stopped, move to STOPPED state | -| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section) | +| Parameter | Type | Description | +| -------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | +| `stop_duration_sec` | double | [s] time parameter for the ego vehicle to stop in front of a stop line | +| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | A flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | +| `show_stop_line_collision_check` | bool | A flag to determine whether to show the debug information of collision check with a stop line | ### Inner-workings / Algorithms - Gets a stop line from map information. - insert a stop point on the path from the stop line defined in the map and the ego vehicle length. - Sets velocities of the path after the stop point to 0[m/s]. -- Release the inserted stop velocity when the vehicle stops within a radius of 2[m] from the stop point. +- Release the inserted stop velocity when the vehicle stops at the stop point for `stop_duration_sec` seconds. #### Flowchart @@ -48,13 +50,13 @@ endif if (state is APPROACH) then (yes) :set stop velocity; - if (vehicle is within stop_check_dist?) then (yes) + if (vehicle is within hold_stop_margin_distance?) then (yes) if (vehicle is stopped?) then (yes) :change state to STOPPED; endif endif else if (state is STOPPED) then (yes) - if (vehicle started to move?) then (yes) + if (stopping time is longer than stop_duration_sec ?) then (yes) :change state to START; endif else if (state is START) then (yes) diff --git a/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml b/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml index 12967d753e25b..7b1d82a46f652 100644 --- a/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml +++ b/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml @@ -2,10 +2,9 @@ ros__parameters: stop_line: stop_margin: 0.0 - stop_check_dist: 2.0 stop_duration_sec: 1.0 use_initialization_stop_line_state: true hold_stop_margin_distance: 2.0 debug: - show_stopline_collision_check: false # [-] whether to show stopline collision + show_stop_line_collision_check: false # [-] whether to show stopline collision diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index c3576db3607f6..09949751f4e93 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -87,7 +87,7 @@ visualization_msgs::msg::MarkerArray createStopLineCollisionCheck( visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - if (planner_param_.show_stopline_collision_check) { + if (planner_param_.show_stop_line_collision_check) { appendMarkerArray( createStopLineCollisionCheck(debug_data_, module_id_), &debug_marker_array, this->clock_->now()); diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 86db87ad602cc..390550fc9e563 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -34,8 +34,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) p.use_initialization_stop_line_state = node.declare_parameter(ns + ".use_initialization_stop_line_state"); // debug - p.show_stopline_collision_check = - node.declare_parameter(ns + ".debug.show_stopline_collision_check"); + p.show_stop_line_collision_check = + node.declare_parameter(ns + ".debug.show_stop_line_collision_check"); } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( diff --git a/planning/behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_stop_line_module/src/scene.hpp index d211d2ef17706..70c67df623c85 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.hpp @@ -74,7 +74,7 @@ class StopLineModule : public SceneModuleInterface double stop_duration_sec; double hold_stop_margin_distance; bool use_initialization_stop_line_state; - bool show_stopline_collision_check; + bool show_stop_line_collision_check; }; public: diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 381b8b472adfa..afe345490201b 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -350,6 +350,7 @@ bool CostmapGenerator::isActive() return false; } else { const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger()); + if (!current_pose_wrt_map) return false; return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose); } } diff --git a/planning/freespace_planner/config/freespace_planner.param.yaml b/planning/freespace_planner/config/freespace_planner.param.yaml index 75c813266a83d..8692aa293cbf7 100644 --- a/planning/freespace_planner/config/freespace_planner.param.yaml +++ b/planning/freespace_planner/config/freespace_planner.param.yaml @@ -33,3 +33,11 @@ only_behind_solutions: false use_back: true distance_heuristic_weight: 1.0 + + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 0.1 diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 6f1810fabec88..3dced3eb1d01d 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -118,9 +118,9 @@ class AstarSearch : public AbstractPlanningAlgorithm : AstarSearch( planner_common_param, collision_vehicle_shape, AstarParam{ - node.declare_parameter("astar.only_behind_solutions", false), - node.declare_parameter("astar.use_back", true), - node.declare_parameter("astar.distance_heuristic_weight", 1.0)}) + node.declare_parameter("astar.only_behind_solutions"), + node.declare_parameter("astar.use_back"), + node.declare_parameter("astar.distance_heuristic_weight")}) { } diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp index 07f73281e7176..ac21f7333df37 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp @@ -47,11 +47,11 @@ class RRTStar : public AbstractPlanningAlgorithm : RRTStar( planner_common_param, original_vehicle_shape, RRTStarParam{ - node.declare_parameter("rrtstar.enable_update", true), - node.declare_parameter("rrtstar.use_informed_sampling", true), - node.declare_parameter("rrtstar.max_planning_time", 150.0), - node.declare_parameter("rrtstar.neighbor_radius", 8.0), - node.declare_parameter("rrtstar.margin", 0.1)}) + node.declare_parameter("rrtstar.enable_update"), + node.declare_parameter("rrtstar.use_informed_sampling"), + node.declare_parameter("rrtstar.max_planning_time"), + node.declare_parameter("rrtstar.neighbor_radius"), + node.declare_parameter("rrtstar.margin")}) { } diff --git a/planning/freespace_planning_algorithms/test/debug_plot.py b/planning/freespace_planning_algorithms/test/debug_plot.py index bc96963f09dcb..a34fd745af047 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot.py +++ b/planning/freespace_planning_algorithms/test/debug_plot.py @@ -223,6 +223,6 @@ def create_concat_png(src_list, dest, is_horizontal): plt.savefig(file_name) print("saved to {}".format(file_name)) - algowise_summary_file = os.path.join("/tmp", "summary-{}.png".format(algo_name)) + algo_summary_file = os.path.join("/tmp", "summary-{}.png".format(algo_name)) if concat: - create_concat_png(algo_png_images, algowise_summary_file, True) + create_concat_png(algo_png_images, algo_summary_file, True) diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index 4eb91e423c01f..3b649168884e6 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -14,16 +14,17 @@ In current Autoware.universe, only Lanelet2 map format is supported. ### Parameters -| Name | Type | Description | -| -------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- | -| `map_frame` | string | The frame name for map | -| `arrival_check_angle_deg` | double | Angle threshold for goal check | -| `arrival_check_distance` | double | Distance threshold for goal check | -| `arrival_check_duration` | double | Duration threshold for goal check | -| `goal_angle_threshold` | double | Max goal pose angle for goal approve | -| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | -| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | -| `minimum_reroute_length` | double | Minimum Length for publishing a new route | +| Name | Type | Description | +| ---------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------- | +| `map_frame` | string | The frame name for map | +| `arrival_check_angle_deg` | double | Angle threshold for goal check | +| `arrival_check_distance` | double | Distance threshold for goal check | +| `arrival_check_duration` | double | Duration threshold for goal check | +| `goal_angle_threshold` | double | Max goal pose angle for goal approve | +| `enable_correct_goal_pose` | bool | Enabling correction of goal pose according to the closest lanelet orientation | +| `reroute_time_threshold` | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible | +| `minimum_reroute_length` | double | Minimum Length for publishing a new route | +| `consider_no_drivable_lanes` | bool | This flag is for considering no_drivable_lanes in planning or not. | ### Services diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml index d596eb9816f0a..98c28344ea25c 100644 --- a/planning/mission_planner/config/mission_planner.param.yaml +++ b/planning/mission_planner/config/mission_planner.param.yaml @@ -8,3 +8,4 @@ enable_correct_goal_pose: false reroute_time_threshold: 10.0 minimum_reroute_length: 30.0 + consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f5f7eefd49d7d..49e1d0de8be93 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -33,6 +33,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_planning_msgs vehicle_info_util ament_lint_auto diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 95a4798e7db68..d8289824e517c 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -147,6 +147,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); param_.enable_correct_goal_pose = node_->declare_parameter("enable_correct_goal_pose"); + param_.consider_no_drivable_lanes = node_->declare_parameter("consider_no_drivable_lanes"); } void DefaultPlanner::initialize(rclcpp::Node * node) @@ -401,7 +402,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) const auto goal_check_point = points.at(i); lanelet::ConstLanelets path_lanelets; if (!route_handler_.planPathLaneletsBetweenCheckpoints( - start_check_point, goal_check_point, &path_lanelets)) { + start_check_point, goal_check_point, &path_lanelets, param_.consider_no_drivable_lanes)) { RCLCPP_WARN(logger, "Failed to plan route."); return route_msg; } diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 09b519267c962..cf5a19443dd82 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -37,6 +37,7 @@ struct DefaultPlannerParameters { double goal_angle_threshold_deg; bool enable_correct_goal_pose; + bool consider_no_drivable_lanes; }; class DefaultPlanner : public mission_planner::PlannerPlugin diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 0c9b27a685583..08da1260b000c 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -75,6 +75,9 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), + odometry_(nullptr), + map_ptr_(nullptr), + reroute_availability_(nullptr), normal_route_(nullptr), mrm_route_(nullptr) { @@ -85,15 +88,19 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - odometry_ = nullptr; sub_odometry_ = create_subscription( "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); + sub_reroute_availability_ = create_subscription( + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/" + "is_reroute_available", + rclcpp::QoS(1), + std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); auto qos_transient_local = rclcpp::QoS{1}.transient_local(); - vector_map_subscriber_ = create_subscription( + sub_vector_map_ = create_subscription( "input/vector_map", qos_transient_local, - std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); @@ -130,7 +137,12 @@ void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) } } -void MissionPlanner::onMap(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg) +{ + reroute_availability_ = msg; +} + +void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) { map_ptr_ = msg; } @@ -375,6 +387,10 @@ void MissionPlanner::on_set_mrm_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } const auto prev_state = state_.state; change_state(RouteState::Message::CHANGING); @@ -444,6 +460,10 @@ void MissionPlanner::on_clear_mrm_route( if (!mrm_route_) { throw component_interface_utils::NoEffectWarning("MRM route is not set"); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); @@ -574,6 +594,10 @@ void MissionPlanner::on_change_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } // set to changing state change_state(RouteState::Message::CHANGING); @@ -633,6 +657,10 @@ void MissionPlanner::on_change_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); } + if (reroute_availability_ && !reroute_availability_->availability) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + } change_state(RouteState::Message::CHANGING); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 4d09f72f81899..fd1b317970749 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -55,6 +56,7 @@ using NormalRoute = planning_interface::NormalRoute; using MrmRoute = planning_interface::MrmRoute; using RouteState = planning_interface::RouteState; using Odometry = nav_msgs::msg::Odometry; +using RerouteAvailability = tier4_planning_msgs::msg::RerouteAvailability; class MissionPlanner : public rclcpp::Node { @@ -71,9 +73,14 @@ class MissionPlanner : public rclcpp::Node PoseStamped transform_pose(const PoseStamped & input); rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_reroute_availability_; Odometry::ConstSharedPtr odometry_; + HADMapBin::ConstSharedPtr map_ptr_; + RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_map(const HADMapBin::ConstSharedPtr msg); + void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -121,9 +128,6 @@ class MissionPlanner : public rclcpp::Node const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; - void onMap(const HADMapBin::ConstSharedPtr msg); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml index 78bddc2b81f4f..86f767560fd7c 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml @@ -13,13 +13,13 @@ - + - + diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index b77af629ad7c3..d3c77aa0faa84 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -62,11 +62,12 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions "~/input/external_velocity_limit_mps", 1, std::bind(&MotionVelocitySmootherNode::onExternalVelocityLimit, this, _1)); sub_current_acceleration_ = create_subscription( - "~/input/acceleration", 1, - [this](const AccelWithCovarianceStamped::SharedPtr msg) { current_acceleration_ptr_ = msg; }); + "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { + current_acceleration_ptr_ = msg; + }); sub_operation_mode_ = create_subscription( "~/input/operation_mode_state", 1, - [this](const OperationModeState::SharedPtr msg) { operation_mode_ = *msg; }); + [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update set_param_res_ = this->add_on_set_parameters_callback( diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 3d72d6e4b5c83..2bcb31ff10969 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -222,38 +222,38 @@ bool calcStopVelocityWithConstantJerkAccLimit( return true; } - double dist = 0.0; - std::vector dists; - dists.push_back(dist); + double distance = 0.0; + std::vector distances; + distances.push_back(distance); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { - dist += + distance += tier4_autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); - if (dist > xs.back()) { + if (distance > xs.back()) { break; } - dists.push_back(dist); + distances.push_back(distance); } if ( - !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(dists) || - !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(dists)) { + !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distances) || + !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distances)) { return false; } if ( - xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || dists.empty() || - dists.front() < xs.front() || xs.back() < dists.back()) { + xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || distances.empty() || + distances.front() < xs.front() || xs.back() < distances.back()) { return false; } - const auto vel_at_wp = interpolation::lerp(xs, vs, dists); - const auto acc_at_wp = interpolation::lerp(xs, as, dists); - const auto jerk_at_wp = interpolation::lerp(xs, js, dists); + const auto vel_at_wp = interpolation::lerp(xs, vs, distances); + const auto acc_at_wp = interpolation::lerp(xs, as, distances); + const auto jerk_at_wp = interpolation::lerp(xs, js, distances); // for debug std::stringstream ssi; - for (unsigned int i = 0; i < dists.size(); ++i) { - ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) + for (unsigned int i = 0; i < distances.size(); ++i) { + ssi << "d: " << distances.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) << ", j: " << jerk_at_wp.at(i) << std::endl; } RCLCPP_DEBUG( diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 65b4c9fab3d21..bb64006656216 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: option: - enable_skip_optimization: false # skip model predictive trajectory + enable_skip_optimization: false # skip elastic band and model predictive trajectory enable_reset_prev_optimization: false # If true, optimization has no fix constraint to the previous result. enable_outside_drivable_area_stop: true # stop if the ego's trajectory footprint is outside the drivable area use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered. @@ -9,6 +9,7 @@ debug: # flag to publish enable_pub_debug_marker: true # publish debug marker + enable_pub_extra_debug_marker: false # publish extra debug marker # flag to show enable_debug_info: false @@ -19,7 +20,7 @@ output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] - vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . + vehicle_stop_margin_outside_drivable_area: 0.0 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . # This margin will be realized with delta_arc_length_for_mpt_points m precision. # replanning & trimming trajectory param outside algorithm @@ -77,7 +78,7 @@ # avoidance avoidance: - max_bound_fixing_time: 3.0 # [s] + max_bound_fixing_time: 1.0 # [s] max_longitudinal_margin_for_bound_violation: 1.0 # [m] max_avoidance_cost: 0.5 # [m] avoidance_cost_margin: 0.0 # [m] diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp index 8753699ab9d4b..ce12cbca8ddb6 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_marker.hpp @@ -30,6 +30,6 @@ namespace obstacle_avoidance_planner MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info); + const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); } // namespace obstacle_avoidance_planner #endif // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_MARKER_HPP_ diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index ca51c791b8f0c..ea2e18b099f1d 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -118,6 +118,7 @@ class MPTOptimizer void onParam(const std::vector & parameters); double getTrajectoryLength() const; + double getDeltaArcLength() const; int getNumberOfPoints() const; private: @@ -237,6 +238,7 @@ class MPTOptimizer // previous data int prev_mat_n_ = 0; int prev_mat_m_ = 0; + int prev_solution_status_ = 0; std::shared_ptr> prev_ref_points_ptr_{nullptr}; std::shared_ptr> prev_optimized_traj_points_ptr_{nullptr}; @@ -295,7 +297,7 @@ class MPTOptimizer const std::vector & ref_points) const; std::optional> calcMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & U, + std::vector & ref_points, const Eigen::VectorXd & optimized_variables, const StateEquationGenerator::Matrix & mpt_matrix) const; void publishDebugTrajectories( diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 68e9733bcc12f..f716f497b90da 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -61,6 +61,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node // flags for some functions bool enable_pub_debug_marker_; + bool enable_pub_extra_debug_marker_; bool enable_debug_info_; bool enable_outside_drivable_area_stop_; bool enable_skip_optimization_; @@ -76,7 +77,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node EgoNearestParam ego_nearest_param_{}; // variables for subscribers - Odometry::SharedPtr ego_state_ptr_; + Odometry::ConstSharedPtr ego_state_ptr_; // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; @@ -97,7 +98,7 @@ class ObstacleAvoidancePlanner : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; // subscriber callback function - void onPath(const Path::SharedPtr); + void onPath(const Path::ConstSharedPtr path_ptr); // reset functions void initializePlanning(); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp index d4f70e8e09227..b6f4eadb682fd 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/state_equation_generator.hpp @@ -31,6 +31,7 @@ class StateEquationGenerator public: struct Matrix { + Eigen::MatrixXd A; Eigen::MatrixXd B; Eigen::VectorXd W; }; diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py index 042c2ff5f0345..daf6beea730d3 100755 --- a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py +++ b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py @@ -28,6 +28,7 @@ def __init__(self, args): super().__init__("calculation_cost_analyzer") self.functions_name = args.functions.split(",") + self.depth = args.depth self.calculation_cost_hist = [] self.sub_calculation_cost = self.create_subscription( @@ -67,7 +68,7 @@ def CallbackCalculationCost(self, msg): if not is_found: self.y_vec[f_idx].append(None) - if len(self.y_vec[f_idx]) > 100: + if len(self.y_vec[f_idx]) > self.depth: self.y_vec[f_idx].popleft() x_vec = list(range(len(self.y_vec[f_idx]))) @@ -99,7 +100,13 @@ def CallbackCalculationCost(self, msg): "-f", "--functions", type=str, - default="onPath, getModelPredictiveTrajectory, getEBTrajectory", + default="onPath, calcReferencePoints, calcOptimizedSteerAngles, publishDebugMarkerOfOptimization", + ) + parser.add_argument( + "-d", + "--depth", + type=float, + default=500, ) args = parser.parse_args() diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index ffe6444151cd4..79f3dfd2fe2e4 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -364,16 +364,11 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info) + const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker) { MarkerArray marker_array; - // mpt footprints - appendMarkerArray( - getFootprintsMarkerArray(optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), - &marker_array); - - // bounds lines + // bounds line appendMarkerArray( getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); @@ -383,13 +378,6 @@ MarkerArray getDebugMarker( debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), &marker_array); - // vehicle circle line - appendMarkerArray( - getVehicleCircleLinesMarkerArray( - debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, - vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), - &marker_array); - // current vehicle circles appendMarkerArray( getCurrentVehicleCirclesMarkerArray( @@ -397,25 +385,43 @@ MarkerArray getDebugMarker( debug_data.vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3), &marker_array); - // vehicle circles - appendMarkerArray( - getVehicleCirclesMarkerArray( - optimized_points, debug_data.vehicle_circle_longitudinal_offsets, - debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, "vehicle_circles", - 1.0, 0.3, 0.3), - &marker_array); + // NOTE: Default debug marker is limited for less calculation time + // Circles visualization is comparatively heavy. + if (publish_extra_marker) { + // vehicle circles + appendMarkerArray( + getVehicleCirclesMarkerArray( + optimized_points, debug_data.vehicle_circle_longitudinal_offsets, + debug_data.vehicle_circle_radiuses, debug_data.mpt_visualize_sampling_num, + "vehicle_circles", 1.0, 0.3, 0.3), + &marker_array); - // footprint by drivable area - if (debug_data.stop_pose_by_drivable_area) { + // mpt footprints appendMarkerArray( - getFootprintByDrivableAreaMarkerArray( - *debug_data.stop_pose_by_drivable_area, vehicle_info, "footprint_by_drivable_area", 1.0, - 0.0, 0.0), + getFootprintsMarkerArray( + optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); - } - // debug text - appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); + // vehicle circle line + appendMarkerArray( + getVehicleCircleLinesMarkerArray( + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, + vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, + "vehicle_circle_lines"), + &marker_array); + + // footprint by drivable area + if (debug_data.stop_pose_by_drivable_area) { + appendMarkerArray( + getFootprintByDrivableAreaMarkerArray( + *debug_data.stop_pose_by_drivable_area, vehicle_info, "footprint_by_drivable_area", 1.0, + 0.0, 0.0), + &marker_array); + } + + // debug text + appendMarkerArray(getPointsTextMarkerArray(debug_data.ref_points), &marker_array); + } return marker_array; } diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 81b5974ac3deb..0a55cc8d91f8e 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -497,15 +497,15 @@ std::vector MPTOptimizer::optimizeTrajectory( const auto const_mat = calcConstraintMatrix(mpt_mat, ref_points); // 6. optimize steer angles - const auto optimized_steer_angles = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); - if (!optimized_steer_angles) { + const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat); + if (!optimized_variables) { RCLCPP_INFO_EXPRESSION( logger_, enable_debug_info_, "return std::nullopt since could not solve qp"); return get_prev_optimized_traj_points(); } // 7. convert to points with validation - const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_steer_angles, mpt_mat); + const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat); if (!mpt_traj_points) { RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large."); return get_prev_optimized_traj_points(); @@ -687,6 +687,8 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { + time_keeper_ptr_->tic(__func__); + // alpha for (size_t i = 0; i < ref_points.size(); ++i) { const auto front_wheel_pos = @@ -774,6 +776,8 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c } } } + + time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateBounds( @@ -1160,13 +1164,14 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; const bool is_goal_contained = geometry_utils::isSamePoint(ref_points.back(), traj_points.back()); // update Q - Eigen::SparseMatrix Q_sparse_mat(D_x * N_ref, D_x * N_ref); std::vector> Q_triplet_vec; for (size_t i = 0; i < N_ref; ++i) { const auto adaptive_error_weight = [&]() -> std::array { @@ -1198,51 +1203,50 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( Q_triplet_vec.push_back( Eigen::Triplet(i * D_x + 1, i * D_x + 1, adaptive_yaw_error_weight)); } + Eigen::SparseMatrix Q_sparse_mat(N_x, N_x); Q_sparse_mat.setFromTriplets(Q_triplet_vec.begin(), Q_triplet_vec.end()); // update R - Eigen::SparseMatrix R_sparse_mat(D_v, D_v); std::vector> R_triplet_vec; for (size_t i = 0; i < N_ref - 1; ++i) { const double adaptive_steer_weight = interpolation::lerp( mpt_param_.steer_input_weight, mpt_param_.avoidance_steer_input_weight, ref_points.at(i).normalized_avoidance_cost); - R_triplet_vec.push_back( - Eigen::Triplet(D_x + D_u * i, D_x + D_u * i, adaptive_steer_weight)); + R_triplet_vec.push_back(Eigen::Triplet(D_u * i, D_u * i, adaptive_steer_weight)); } + Eigen::SparseMatrix R_sparse_mat(N_u, N_u); addSteerWeightR(R_triplet_vec, ref_points); R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); - ValueMatrix m; - m.Q = Q_sparse_mat; - m.R = R_sparse_mat; - time_keeper_ptr_->toc(__func__, " "); - return m; + return ValueMatrix{Q_sparse_mat, R_sparse_mat}; } MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( - const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, + [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { time_keeper_ptr_->tic(__func__); - const size_t N_ref = ref_points.size(); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); - const size_t D_xn = D_x * N_ref; - const size_t D_v = D_x + (N_ref - 1) * D_u; + + const size_t N_ref = ref_points.size(); const size_t N_slack = getNumberOfSlackVariables(); + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; + const size_t N_s = N_ref * N_slack; + + const size_t N_v = N_x + N_u + N_s; + // generate T matrix and vector to shift optimization center // NOTE: Z is defined as time-series vector of shifted deviation - // error where Z = sparse_T_mat * (B * U + W) + T_vec - Eigen::SparseMatrix sparse_T_mat(D_xn, D_xn); - Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(D_xn); + // error where Z = sparse_T_mat * X + T_vec std::vector> triplet_T_vec; + Eigen::VectorXd T_vec = Eigen::VectorXd::Zero(N_x); const double offset = mpt_param_.optimization_center_offset; - for (size_t i = 0; i < N_ref; ++i) { const double alpha = ref_points.at(i).alpha; @@ -1252,40 +1256,28 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( T_vec(i * D_x) = -offset * std::sin(alpha); } + Eigen::SparseMatrix sparse_T_mat(N_x, N_x); sparse_T_mat.setFromTriplets(triplet_T_vec.begin(), triplet_T_vec.end()); - const Eigen::MatrixXd B = sparse_T_mat * mpt_mat.B; - const Eigen::MatrixXd QB = val_mat.Q * B; - const Eigen::MatrixXd R = val_mat.R; - - // calculate H, and extend it for slack variables // NOTE: min J(v) = min (v'Hv + v'g) - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(D_v, D_v); - H.triangularView() = B.transpose() * QB + R; - H.triangularView() = H.transpose(); + Eigen::MatrixXd H_x = Eigen::MatrixXd::Zero(N_x, N_x); + H_x.triangularView() = + Eigen::MatrixXd(sparse_T_mat.transpose() * val_mat.Q * sparse_T_mat); + H_x.triangularView() = H_x.transpose(); - Eigen::MatrixXd extended_H = Eigen::MatrixXd::Zero(D_v + N_ref * N_slack, D_v + N_ref * N_slack); - extended_H.block(0, 0, D_v, D_v) = H; + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(N_v, N_v); + H.block(0, 0, N_x, N_x) = H_x; + H.block(N_x, N_x, N_u, N_u) = val_mat.R; - // calculate g, and extend it for slack variables - Eigen::VectorXd g = (sparse_T_mat * mpt_mat.W + T_vec).transpose() * QB; - /* - Eigen::VectorXd extended_g(D_v + N_ref * N_slack); - - extended_g.segment(0, D_v) = g; - if (N_slack > 0) { - extended_g.segment(D_v, N_ref * N_slack) = - mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); - } - */ - Eigen::VectorXd extended_g(D_v + N_ref * N_slack); - extended_g << g, mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_ref * N_slack); + Eigen::VectorXd g = Eigen::VectorXd::Zero(N_v); + g.segment(0, N_x) = T_vec.transpose() * val_mat.Q * sparse_T_mat; + g.segment(N_x + N_u, N_s) = mpt_param_.soft_collision_free_weight * Eigen::VectorXd::Ones(N_s); ObjectiveMatrix obj_matrix; - obj_matrix.hessian = extended_H; - obj_matrix.gradient = extended_g; + obj_matrix.hessian = H; + obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); + time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1300,15 +1292,19 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); + const size_t N_x = N_ref * D_x; const size_t N_u = (N_ref - 1) * D_u; - const size_t D_v = D_x + N_u; - const size_t N_collision_check = vehicle_circle_longitudinal_offsets_.size(); // NOTE: The number of one-step slack variables. // The number of all slack variables will be N_ref * N_slack. const size_t N_slack = getNumberOfSlackVariables(); + const size_t N_v = N_x + N_u + (mpt_param_.soft_constraint ? N_ref * N_slack : 0); + + const size_t N_collision_check = vehicle_circle_longitudinal_offsets_.size(); + // calculate indices of fixed points std::vector fixed_points_indices; for (size_t i = 0; i < N_ref; ++i) { @@ -1319,6 +1315,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( // calculate rows and cols of A size_t A_rows = 0; + A_rows += N_x; if (mpt_param_.soft_constraint) { // NOTE: 3 means expecting slack variable constraints to be larger than lower bound, // smaller than upper bound, and positive. @@ -1332,22 +1329,24 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows += N_u; } - const size_t A_cols = [&] { - if (mpt_param_.soft_constraint) { - return D_v + N_ref * N_slack; // initial state + steer angles + soft variables - } - return D_v; // initial state + steer angles - }(); - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, A_cols); + // NOTE: The following takes 1 [ms] + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, N_v); Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); size_t A_rows_end = 0; + // 1. State equation + A.block(0, 0, N_x, N_x) = Eigen::MatrixXd::Identity(N_x, N_x) - mpt_mat.A; + A.block(0, N_x, N_x, N_u) = -mpt_mat.B; + lb.segment(0, N_x) = mpt_mat.W; + ub.segment(0, N_x) = mpt_mat.W; + A_rows_end += N_x; + + // 2. collision free // CX = C(Bv + w) + C \in R^{N_ref, N_ref * D_x} for (size_t l_idx = 0; l_idx < N_collision_check; ++l_idx) { // create C := [cos(beta) | l cos(beta)] - Eigen::SparseMatrix C_sparse_mat(N_ref, N_ref * D_x); + Eigen::SparseMatrix C_sparse_mat(N_ref, N_x); std::vector> C_triplet_vec; Eigen::VectorXd C_vec = Eigen::VectorXd::Zero(N_ref); @@ -1362,10 +1361,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( } C_sparse_mat.setFromTriplets(C_triplet_vec.begin(), C_triplet_vec.end()); - // calculate CB, and CW - const Eigen::MatrixXd CB = C_sparse_mat * mpt_mat.B; - const Eigen::VectorXd CW = C_sparse_mat * mpt_mat.W + C_vec; - // calculate bounds const double bounds_offset = vehicle_info_.vehicle_width_m / 2.0 - vehicle_circle_radiuses_.at(l_idx); @@ -1373,36 +1368,30 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( // soft constraints if (mpt_param_.soft_constraint) { - size_t A_offset_cols = D_v; const size_t A_blk_rows = 3 * N_ref; - // A := [C * B | O | ... | O | I | O | ... - // -C * B | O | ... | O | I | O | ... + // A := [C | O | ... | O | I | O | ... + // -C | O | ... | O | I | O | ... // O | O | ... | O | I | O | ... ] - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); - A_blk.block(0, 0, N_ref, D_v) = CB; - A_blk.block(N_ref, 0, N_ref, D_v) = -CB; + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, N_v); + A_blk.block(0, 0, N_ref, N_x) = C_sparse_mat; + A_blk.block(N_ref, 0, N_ref, N_x) = -C_sparse_mat; - size_t local_A_offset_cols = A_offset_cols; - if (!mpt_param_.l_inf_norm) { - local_A_offset_cols += N_ref * l_idx; - } + const size_t local_A_offset_cols = N_x + N_u + (!mpt_param_.l_inf_norm ? N_ref * l_idx : 0); A_blk.block(0, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); A_blk.block(N_ref, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); A_blk.block(2 * N_ref, local_A_offset_cols, N_ref, N_ref) = Eigen::MatrixXd::Identity(N_ref, N_ref); - // lb := [lower_bound - CW - // CW - upper_bound + // lb := [lower_bound - C + // C - upper_bound // O ] Eigen::VectorXd lb_blk = Eigen::VectorXd::Zero(A_blk_rows); - lb_blk.segment(0, N_ref) = -CW + part_lb; - lb_blk.segment(N_ref, N_ref) = CW - part_ub; + lb_blk.segment(0, N_ref) = -C_vec + part_lb; + lb_blk.segment(N_ref, N_ref) = C_vec - part_ub; - A_offset_cols += N_ref * N_slack; - - A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; + A.block(A_rows_end, 0, A_blk_rows, N_v) = A_blk; lb.segment(A_rows_end, A_blk_rows) = lb_blk; A_rows_end += A_blk_rows; @@ -1412,33 +1401,31 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( if (mpt_param_.hard_constraint) { const size_t A_blk_rows = N_ref; - Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, A_cols); - A_blk.block(0, 0, N_ref, N_ref) = CB; + Eigen::MatrixXd A_blk = Eigen::MatrixXd::Zero(A_blk_rows, N_v); + A_blk.block(0, 0, N_ref, N_ref) = C_sparse_mat; - A.block(A_rows_end, 0, A_blk_rows, A_cols) = A_blk; - lb.segment(A_rows_end, A_blk_rows) = part_lb - CW; - ub.segment(A_rows_end, A_blk_rows) = part_ub - CW; + A.block(A_rows_end, 0, A_blk_rows, N_v) = A_blk; + lb.segment(A_rows_end, A_blk_rows) = part_lb - C_vec; + ub.segment(A_rows_end, A_blk_rows) = part_ub - C_vec; A_rows_end += A_blk_rows; } } - // fixed points constraint + // 3. fixed points constraint // X = B v + w where point is fixed for (const size_t i : fixed_points_indices) { - A.block(A_rows_end, 0, D_x, D_v) = mpt_mat.B.block(i * D_x, 0, D_x, D_v); + A.block(A_rows_end, D_x * i, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); - lb.segment(A_rows_end, D_x) = - ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); - ub.segment(A_rows_end, D_x) = - ref_points[i].fixed_kinematic_state->toEigenVector() - mpt_mat.W.segment(i * D_x, D_x); + lb.segment(A_rows_end, D_x) = ref_points.at(i).fixed_kinematic_state->toEigenVector(); + ub.segment(A_rows_end, D_x) = ref_points.at(i).fixed_kinematic_state->toEigenVector(); A_rows_end += D_x; } - // steer limit + // 4. steer angle limit if (mpt_param_.steer_limit_constraint) { - A.block(A_rows_end, D_x, N_u, N_u) = Eigen::MatrixXd::Identity(N_u, N_u); + A.block(A_rows_end, N_x, N_u, N_u) = Eigen::MatrixXd::Identity(N_u, N_u); // TODO(murooka) use curvature by stabling optimization // Currently, when using curvature, the optimization result is weird with sample_map. @@ -1455,26 +1442,21 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - ConstraintMatrix constraint_matrix; - constraint_matrix.linear = A; - constraint_matrix.lower_bound = lb; - constraint_matrix.upper_bound = ub; - - time_keeper_ptr_->toc(__func__, " "); - return constraint_matrix; + time_keeper_ptr_->toc(__func__, " "); + return ConstraintMatrix{A, lb, ub}; } void MPTOptimizer::addSteerWeightR( std::vector> & R_triplet_vec, const std::vector & ref_points) const { - const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_u = (N_ref - 1) * D_u; // add steering rate : weight for (u(i) - u(i-1))^2 - for (size_t i = D_x; i < D_v - 1; ++i) { + for (size_t i = 0; i < N_u - 1; ++i) { R_triplet_vec.push_back(Eigen::Triplet(i, i, mpt_param_.steer_rate_weight)); R_triplet_vec.push_back(Eigen::Triplet(i + 1, i, -mpt_param_.steer_rate_weight)); R_triplet_vec.push_back(Eigen::Triplet(i, i + 1, -mpt_param_.steer_rate_weight)); @@ -1490,10 +1472,12 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; const size_t N_slack = getNumberOfSlackVariables(); - const size_t D_un = D_v + N_ref * N_slack; + const size_t N_v = N_x + N_u + N_ref * N_slack; // for manual warm start, calculate initial solution const auto u0 = [&]() -> std::optional { @@ -1522,7 +1506,9 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); - if (mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) { + if ( + prev_solution_status_ == 1 && mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && + prev_mat_m_ == A.rows()) { RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "warm start"); osqp_solver_ptr_->updateCscP(P_csc); osqp_solver_ptr_->updateQ(f); @@ -1546,6 +1532,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // check solution status const int solution_status = std::get<3>(result); + prev_solution_status_ = solution_status; if (solution_status != 1) { osqp_solver_ptr_->logUnsolvedStatus("[MPT]"); return std::nullopt; @@ -1565,15 +1552,15 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); return std::nullopt; } - const Eigen::VectorXd optimized_steer_angles = - Eigen::Map(&optimization_result[0], D_un); + const Eigen::VectorXd optimized_variables = + Eigen::Map(&optimization_result[0], N_v); time_keeper_ptr_->toc(__func__, " "); if (u0) { // manual warm start - return static_cast(optimized_steer_angles + *u0); + return static_cast(optimized_variables + *u0); } - return optimized_steer_angles; + return optimized_variables; } Eigen::VectorXd MPTOptimizer::calcInitialSolutionForManualWarmStart( @@ -1649,30 +1636,30 @@ MPTOptimizer::updateMatrixForManualWarmStart( } std::optional> MPTOptimizer::calcMPTPoints( - std::vector & ref_points, const Eigen::VectorXd & U, - const StateEquationGenerator::Matrix & mpt_mat) const + std::vector & ref_points, const Eigen::VectorXd & optimized_variables, + [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { time_keeper_ptr_->tic(__func__); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); + const size_t N_ref = ref_points.size(); - const size_t D_v = D_x + (N_ref - 1) * D_u; + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; const size_t N_slack = getNumberOfSlackVariables(); - const Eigen::VectorXd steer_angles = U.segment(0, D_v); - const Eigen::VectorXd slack_variables = U.segment(D_v, N_ref * N_slack); - - // predict time-series states from optimized control inputs - const Eigen::VectorXd X = state_equation_generator_.predict(mpt_mat, steer_angles); + const Eigen::VectorXd states = optimized_variables.segment(0, N_x); + const Eigen::VectorXd steer_angles = optimized_variables.segment(N_x, N_u); + const Eigen::VectorXd slack_variables = optimized_variables.segment(N_x + N_u, N_ref * N_slack); // calculate trajectory points from optimization result std::vector traj_points; for (size_t i = 0; i < N_ref; ++i) { auto & ref_point = ref_points.at(i); - const double lat_error = X(i * D_x); - const double yaw_error = X(i * D_x + 1); + const double lat_error = states(i * D_x); + const double yaw_error = states(i * D_x + 1); // validate optimization result if (mpt_param_.enable_optimization_validation) { @@ -1688,7 +1675,7 @@ std::optional> MPTOptimizer::calcMPTPoints( if (i == N_ref - 1) { ref_point.optimized_input = 0.0; } else { - ref_point.optimized_input = steer_angles(D_x + i * D_u); + ref_point.optimized_input = steer_angles(i * D_u); } std::vector tmp_slack_variables; @@ -1713,6 +1700,8 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { + time_keeper_ptr_->tic(__func__); + // reference points const auto ref_traj = trajectory_utils::createTrajectory( header, trajectory_utils::convertToTrajectoryPoints(ref_points)); @@ -1726,6 +1715,8 @@ void MPTOptimizer::publishDebugTrajectories( // mpt points const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points); debug_mpt_traj_pub_->publish(mpt_traj); + + time_keeper_ptr_->toc(__func__, " "); } std::vector MPTOptimizer::extractFixedPoints( @@ -1751,6 +1742,11 @@ double MPTOptimizer::getTrajectoryLength() const return forward_traj_length + backward_traj_length; } +double MPTOptimizer::getDeltaArcLength() const +{ + return mpt_param_.delta_arc_length; +} + int MPTOptimizer::getNumberOfPoints() const { return mpt_param_.num_points; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 42a4c7ab78e65..6c4730e9d86f9 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -59,6 +59,17 @@ bool hasZeroVelocity(const TrajectoryPoint & traj_point) constexpr double zero_vel = 0.0001; return std::abs(traj_point.longitudinal_velocity_mps) < zero_vel; } + +std::vector calcSegmentLengthVector(const std::vector & points) +{ + std::vector segment_length_vector; + for (size_t i = 0; i < points.size() - 1; ++i) { + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + segment_length_vector.push_back(segment_length); + } + return segment_length_vector; +} } // namespace ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options) @@ -75,7 +86,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); + "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -94,6 +105,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // parameter for debug marker enable_pub_debug_marker_ = declare_parameter("option.debug.enable_pub_debug_marker"); + enable_pub_extra_debug_marker_ = + declare_parameter("option.debug.enable_pub_extra_debug_marker"); // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); @@ -145,6 +158,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( // parameters for debug marker updateParam(parameters, "option.debug.enable_pub_debug_marker", enable_pub_debug_marker_); + updateParam( + parameters, "option.debug.enable_pub_extra_debug_marker", enable_pub_extra_debug_marker_); // parameters for debug info updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); @@ -189,7 +204,7 @@ void ObstacleAvoidancePlanner::resetPreviousData() mpt_optimizer_ptr_->resetPreviousData(); } -void ObstacleAvoidancePlanner::onPath(const Path::SharedPtr path_ptr) +void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); @@ -363,37 +378,73 @@ void ObstacleAvoidancePlanner::applyInputVelocity( const size_t ego_seg_idx = trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - return motion_utils::cropForwardPoints( + const auto cropped_points = motion_utils::cropForwardPoints( input_traj_points, ego_pose.position, ego_seg_idx, optimized_traj_length + margin_traj_length); + + if (cropped_points.size() < 2) { + return input_traj_points; + } + return cropped_points; }(); // update velocity - size_t input_traj_start_idx = 0; + const auto segment_length_vec = calcSegmentLengthVector(forward_cropped_input_traj_points); + const double mpt_delta_arc_length = mpt_optimizer_ptr_->getDeltaArcLength(); + size_t input_traj_start_idx = trajectory_utils::findEgoSegmentIndex( + forward_cropped_input_traj_points, output_traj_points.front().pose, ego_nearest_param_); for (size_t i = 0; i < output_traj_points.size(); i++) { - // crop backward for efficient calculation - const auto cropped_input_traj_points = std::vector{ - forward_cropped_input_traj_points.begin() + input_traj_start_idx, - forward_cropped_input_traj_points.end()}; + // NOTE: input_traj_start/end_idx is calculated for efficient index calculation + const size_t input_traj_end_idx = [&]() { + double sum_segment_length = 0.0; + for (size_t j = input_traj_start_idx + 1; j < segment_length_vec.size(); ++j) { + sum_segment_length += segment_length_vec.at(j); + if (mpt_delta_arc_length < sum_segment_length) { + return j + 1; + } + } + return forward_cropped_input_traj_points.size() - 1; + }(); + + const auto nearest_traj_point = [&]() { + if (input_traj_start_idx == input_traj_end_idx) { + return forward_cropped_input_traj_points.at(input_traj_start_idx); + } + + // crop forward and backward for efficient calculation + const auto cropped_input_traj_points = std::vector{ + forward_cropped_input_traj_points.begin() + input_traj_start_idx, + forward_cropped_input_traj_points.begin() + input_traj_end_idx + 1}; + assert(2 <= cropped_input_traj_points.size()); - const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( - cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); - input_traj_start_idx = nearest_seg_idx; + const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( + cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); + input_traj_start_idx += nearest_seg_idx; + + return cropped_input_traj_points.at(nearest_seg_idx); + }(); // calculate velocity with zero order hold - const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps; - output_traj_points.at(i).longitudinal_velocity_mps = velocity; + output_traj_points.at(i).longitudinal_velocity_mps = + nearest_traj_point.longitudinal_velocity_mps; } // insert stop point explicitly const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { - const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; - const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( - output_traj_points, input_stop_pose, ego_nearest_param_); + const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // trajectory_utils::findEgoSegmentIndex + // for the case where input_traj_points is much longer than output_traj_points, and the + // former has a stop point but the latter will not have. + const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); + if (stop_seg_idx) { + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); + } } time_keeper_ptr_->toc(__func__, " "); @@ -441,8 +492,8 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( if (first_outside_idx) { debug_data_ptr_->stop_pose_by_drivable_area = optimized_traj_points.at(*first_outside_idx).pose; const auto stop_idx = [&]() { - const auto dist = tier4_autoware_utils::calcDistance2d( - optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_idx)); + const auto dist = + motion_utils::calcSignedArcLength(optimized_traj_points, 0, *first_outside_idx); const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_; const auto first_outside_idx_with_margin = motion_utils::insertTargetPoint(0, dist_with_margin, optimized_traj_points); @@ -492,7 +543,8 @@ void ObstacleAvoidancePlanner::publishDebugMarkerOfOptimization( // debug marker time_keeper_ptr_->tic("getDebugMarker"); - const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_); + const auto debug_marker = + getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_); time_keeper_ptr_->toc("getDebugMarker", " "); time_keeper_ptr_->tic("publishDebugMarker"); diff --git a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp index a6eaf7ffdd2ac..7460ce9c1f764 100644 --- a/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp +++ b/planning/obstacle_avoidance_planner/src/state_equation_generator.cpp @@ -25,31 +25,27 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( { time_keeper_ptr_->tic(__func__); - const size_t N_ref = ref_points.size(); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); - const size_t D_v = D_x + D_u * (N_ref - 1); + + const size_t N_ref = ref_points.size(); + const size_t N_x = N_ref * D_x; + const size_t N_u = (N_ref - 1) * D_u; // matrices for whole state equation - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(D_x * N_ref, D_v); - Eigen::VectorXd W = Eigen::VectorXd::Zero(D_x * N_ref); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N_x, N_x); + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(N_x, N_u); + Eigen::VectorXd W = Eigen::VectorXd::Zero(N_x); // matrices for one-step state equation Eigen::MatrixXd Ad(D_x, D_x); Eigen::MatrixXd Bd(D_x, D_u); Eigen::MatrixXd Wd(D_x, 1); - // calculate one-step state equation considering kinematics N_ref times - for (size_t i = 0; i < N_ref; ++i) { - if (i == 0) { - B.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); - continue; - } - - const int idx_x_i = i * D_x; - const int idx_x_i_prev = (i - 1) * D_x; - const int idx_u_i_prev = (i - 1) * D_u; + A.block(0, 0, D_x, D_x) = Eigen::MatrixXd::Identity(D_x, D_x); + // calculate one-step state equation considering kinematics N_ref times + for (size_t i = 1; i < N_ref; ++i) { // get discrete kinematics matrix A, B, W const auto & p = ref_points.at(i - 1); @@ -59,24 +55,13 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( // p.delta_arc_length); vehicle_model_ptr_->calculateStateEquationMatrix(Ad, Bd, Wd, 0.0, p.delta_arc_length); - B.block(idx_x_i, 0, D_x, D_x) = Ad * B.block(idx_x_i_prev, 0, D_x, D_x); - B.block(idx_x_i, D_x + idx_u_i_prev, D_x, D_u) = Bd; - - for (size_t j = 0; j < i - 1; ++j) { - size_t idx_u_j = j * D_u; - B.block(idx_x_i, D_x + idx_u_j, D_x, D_u) = - Ad * B.block(idx_x_i_prev, D_x + idx_u_j, D_x, D_u); - } - - W.segment(idx_x_i, D_x) = Ad * W.block(idx_x_i_prev, 0, D_x, 1) + Wd; + A.block(i * D_x, (i - 1) * D_x, D_x, D_x) = Ad; + B.block(i * D_x, (i - 1) * D_u, D_x, D_u) = Bd; + W.segment(i * D_x, D_x) = Wd; } - Matrix mat; - mat.B = B; - mat.W = W; - time_keeper_ptr_->toc(__func__, " "); - return mat; + return Matrix{A, B, W}; } Eigen::VectorXd StateEquationGenerator::predict( diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 37d3ab4984501..e2012fab43ba4 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -14,6 +14,8 @@ min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index b6a242609dfff..12ebadf770996 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -165,6 +165,11 @@ struct LongitudinalInfo safe_distance_margin = node.declare_parameter("common.safe_distance_margin"); terminal_safe_distance_margin = node.declare_parameter("common.terminal_safe_distance_margin"); + + hold_stop_velocity_threshold = + node.declare_parameter("common.hold_stop_velocity_threshold"); + hold_stop_distance_threshold = + node.declare_parameter("common.hold_stop_distance_threshold"); } void onParam(const std::vector & parameters) @@ -188,6 +193,11 @@ struct LongitudinalInfo parameters, "common.safe_distance_margin", safe_distance_margin); tier4_autoware_utils::updateParam( parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); + + tier4_autoware_utils::updateParam( + parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold); + tier4_autoware_utils::updateParam( + parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold); } // common parameter @@ -208,6 +218,10 @@ struct LongitudinalInfo // distance margin double safe_distance_margin; double terminal_safe_distance_margin; + + // hold stop + double hold_stop_velocity_threshold; + double hold_stop_distance_threshold; }; struct DebugData diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 1927b266b0186..260a4c6400588 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -235,7 +235,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node } void reset(const std::string & uuid) { counter_.emplace(uuid, 0); } - // NOTE: positive is for meeting entrying condition, and negative is for exiting. + // NOTE: positive is for meeting entering condition, and negative is for exiting. std::unordered_map counter_; std::vector current_uuids_; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 8aeef15c8b97c..9be6ec10e952e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -254,6 +254,11 @@ class PlannerInterface SlowDownParam slow_down_param_; std::vector prev_slow_down_output_; + // previous trajectory and distance to stop + // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or + // crossing lanes. + std::optional, double>> prev_stop_distance_info_{ + std::nullopt}; }; #endif // OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ab2092a3120ca..70c0c687b2514 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,12 +100,13 @@ std::pair projectObstacleVelocityToTrajectory( { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - const double object_yaw = tf2::getYaw(obstacle.pose.orientation); + const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); + const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); return std::make_pair( - obstacle.twist.linear.x * std::cos(object_yaw - traj_yaw), - obstacle.twist.linear.x * std::sin(object_yaw - traj_yaw)); + object_vel_norm * std::cos(object_vel_yaw - traj_yaw), + object_vel_norm * std::sin(object_vel_yaw - traj_yaw)); } double calcObstacleMaxLength(const Shape & shape) @@ -818,7 +819,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return std::nullopt; } - if (std::abs(obstacle.twist.linear.x) < p.outside_obstacle_min_velocity_threshold) { + if ( + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) < + p.outside_obstacle_min_velocity_threshold) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, "[Cruise] Ignore outside obstacle (%s) since the obstacle velocity is low.", @@ -919,8 +922,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle( // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, // and the collision between ego and obstacles are within the margin threshold. const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const double has_high_speed = - p.crossing_obstacle_velocity_threshold < std::abs(obstacle.twist.linear.x); + const double has_high_speed = p.crossing_obstacle_velocity_threshold < + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); if (is_obstacle_crossing && has_high_speed) { // Get highest confidence predicted path const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( @@ -995,7 +998,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } return true; } - // check if entrying slow down + // check if entering slow down if (is_lat_dist_low) { const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); if (p.successive_num_to_entry_slow_down_condition <= count) { @@ -1016,7 +1019,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl const auto obstacle_poly = obstacle.toPolygon(); // calculate collision points with trajectory with lateral stop margin - // NOTE: For additional margin, hysteresis is not devided by two. + // NOTE: For additional margin, hysteresis is not divided by two. const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons( traj_points, vehicle_info_, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 1aaa897986bcd..4cf6bf9e23806 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -228,6 +228,7 @@ std::vector PlannerInterface::generateStopTrajectory( motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; } @@ -244,6 +245,7 @@ std::vector PlannerInterface::generateStopTrajectory( motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + prev_stop_distance_info_ = std::nullopt; return planner_data.traj_points; } @@ -315,9 +317,36 @@ std::vector PlannerInterface::generateStopTrajectory( } // Generate Output Trajectory + const double zero_vel_dist = [&]() { + const double current_zero_vel_dist = + std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle); + + // Hold previous stop distance if necessary + if ( + std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + const double diff_dist_front_points = motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - current_zero_vel_dist) < + longitudinal_info_.hold_stop_distance_threshold) { + return prev_zero_vel_dist; + } + } + + return current_zero_vel_dist; + }(); + + // Insert stop point auto output_traj_points = planner_data.traj_points; - const double zero_vel_dist = - std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle); const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle @@ -338,6 +367,8 @@ std::vector PlannerInterface::generateStopTrajectory( const auto stop_speed_exceeded_msg = createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); + + prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist); } stop_planning_debug_info_.set( diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index bc3e07cf8dd74..fb1e93185f738 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -414,23 +414,27 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( /* get object velocity, and current yaw */ bool get_obj = false; - double obj_vel; - double obj_yaw; + double obj_vel_norm; + double obj_vel_yaw; const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); for (const auto & obj : object_ptr->objects) { const Polygon obj_poly = getPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0, param_.object_polygon_length_margin, param_.object_polygon_width_margin); if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { - obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + obj_vel_yaw = std::atan2( + obj.kinematics.initial_twist_with_covariance.twist.linear.y, + obj.kinematics.initial_twist_with_covariance.twist.linear.x); get_obj = true; break; } } if (get_obj) { - *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + *velocity = obj_vel_norm * std::cos(obj_vel_yaw - traj_yaw); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; return true; } else { @@ -442,10 +446,15 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( const PredictedObject & object, const double traj_yaw, double * velocity) { /* get object velocity, and current yaw */ - double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); - - *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw)); + double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + double obj_vel_yaw = std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); + + *velocity = + obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; } diff --git a/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp index cfd8ea0f3e244..936476c800117 100644 --- a/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp +++ b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp index c33e283ed7c0c..aea169d6ce2e0 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -42,8 +41,8 @@ visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, co /// @param[in] marker_z z-value to use for markers /// @return marker array visualization_msgs::msg::MarkerArray makeDebugMarkers( - const Obstacles & obstacles, const std::vector & original_projections, - const std::vector & adjusted_projections, + const Obstacles & obstacles, const std::vector & original_projections, + const std::vector & adjusted_projections, const std::vector & original_footprints, const std::vector & adjusted_footprints, const ObstacleMasks & obstacle_masks, const Float marker_z); diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp index ca80fc468079d..22d70463fc0d9 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp @@ -18,7 +18,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp index 51b5111b9d6ac..3f5f8cd814ea3 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -36,7 +35,7 @@ segment_t forwardSimulatedSegment( /// @param [in] origin origin of the projection /// @param [in] params parameters of the forward projection /// @return lines from the origin to its positions after forward projection, ordered left to right -multilinestring_t bicycleProjectionLines( +multi_linestring_t bicycleProjectionLines( const geometry_msgs::msg::Point & origin, const ProjectionParameters & params); /// @brief generate projection line using the bicycle model @@ -64,7 +63,7 @@ polygon_t generateFootprint(const linestring_t & linestring, const double latera /// @param [in] lines linestring from which to create the footprint /// @param [in] lateral_offset offset around the segment used to create the footprint /// @return footprint polygon -polygon_t generateFootprint(const multilinestring_t & lines, const double lateral_offset); +polygon_t generateFootprint(const multi_linestring_t & lines, const double lateral_offset); /// @brief generate a footprint from a left, right, and middle linestrings and a lateral offset /// @param [in] left left linestring diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp index 32b69b149e9f2..94b7f2425a670 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_VELOCITY_LIMITER__MAP_UTILS_HPP_ #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -30,7 +29,7 @@ namespace obstacle_velocity_limiter /// @param[in] lanelet_map lanelet map /// @param[in] tags tags to identify obstacle linestrings /// @return the extracted obstacles -multilinestring_t extractStaticObstacles( +multi_linestring_t extractStaticObstacles( const lanelet::LaneletMap & lanelet_map, const std::vector & tags); /// @brief Determine if the given linestring is an obstacle diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index 91611938119e5..4d89d4b440c21 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -18,7 +18,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -63,7 +62,7 @@ void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); /// @param[in] buffer buffer used to enlarge the mask /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects -multipolygon_t createPolygonMasks( +multi_polygon_t createPolygonMasks( const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, const Float min_vel); @@ -74,7 +73,7 @@ multipolygon_t createPolygonMasks( /// @param[in] lateral_offset offset to create polygons around the lines /// @return polygon footprint of each projection lines std::vector createFootprintPolygons( - const std::vector & projected_linestrings, const Float lateral_offset); + const std::vector & projected_linestrings, const Float lateral_offset); /// @brief create the footprint polygon from a trajectory /// @param[in] trajectory the trajectory for which to create a footprint @@ -104,7 +103,7 @@ polygon_t createEnvelopePolygon(const std::vector & footprints); /// @param[in] trajectory input trajectory /// @param[in] params projection parameters /// @return projection lines for each trajectory point -std::vector createProjectedLines( +std::vector createProjectedLines( const Trajectory & trajectory, ProjectionParameters & params); /// @brief limit the velocity of the given trajectory @@ -116,7 +115,7 @@ std::vector createProjectedLines( /// @param[in] velocity_params velocity parameters void limitVelocity( Trajectory & trajectory, const CollisionChecker & collision_checker, - const std::vector & projections, const std::vector & footprints, + const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params); /// @brief copy the velocity profile of a downsampled trajectory to the original trajectory diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 13fac28291c3d..8916137c84077 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -18,7 +18,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -75,7 +74,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node OccupancyGrid::ConstSharedPtr occupancy_grid_ptr_; PointCloud::ConstSharedPtr pointcloud_ptr_; lanelet::LaneletMapPtr lanelet_map_ptr_{new lanelet::LaneletMap}; - multilinestring_t static_map_obstacles_; + multi_linestring_t static_map_obstacles_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_; // parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp index 0d7edc649857d..8620c254968fe 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -41,7 +40,7 @@ namespace obstacle_velocity_limiter struct Obstacles { - multilinestring_t lines; + multi_linestring_t lines; multipoint_t points; }; @@ -69,11 +68,11 @@ struct ObstacleTree }; template <> -struct ObstacleTree +struct ObstacleTree { bgi::rtree> segments_rtree; - explicit ObstacleTree(const multilinestring_t & obstacles) + explicit ObstacleTree(const multi_linestring_t & obstacles) { auto segment_count = 0lu; for (const auto & line : obstacles) @@ -115,7 +114,7 @@ struct CollisionChecker { const Obstacles obstacles; std::unique_ptr> point_obstacle_tree_ptr; - std::unique_ptr> line_obstacle_tree_ptr; + std::unique_ptr> line_obstacle_tree_ptr; explicit CollisionChecker( Obstacles obs, const size_t rtree_min_points, const size_t rtree_min_segments) @@ -125,7 +124,7 @@ struct CollisionChecker for (const auto & line : obstacles.lines) if (!line.empty()) segment_count += line.size() - 1; if (segment_count > rtree_min_segments) - line_obstacle_tree_ptr = std::make_unique>(obstacles.lines); + line_obstacle_tree_ptr = std::make_unique>(obstacles.lines); if (obstacles.points.size() > rtree_min_points) point_obstacle_tree_ptr = std::make_unique>(obstacles.points); } @@ -163,7 +162,7 @@ polygon_t createObjectPolygon( /// @param [in] buffer buffer to add to the objects dimensions /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects -multipolygon_t createObjectPolygons( +multi_polygon_t createObjectPolygons( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity); diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp index 61f4f575f826b..b99407ae97846 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -42,7 +41,7 @@ grid_map::GridMap convertToGridMap(const OccupancyGrid & occupancy_grid); /// @param[in] occupancy_grid input occupancy grid /// @param[in] occupied_threshold threshold to use for identifying obstacles in the occupancy grid /// @return extracted obstacle linestrings -multilinestring_t extractObstacles( +multi_linestring_t extractObstacles( const grid_map::GridMap & grid_map, const OccupancyGrid & occupancy_grid); } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp index 7521e6b1a0bba..def328c8c9ae7 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_VELOCITY_LIMITER__PARAMETERS_HPP_ #include -// cspell: ignore multipolygon, multilinestring #include #include @@ -40,8 +39,7 @@ struct ObstacleParameters static constexpr auto RTREE_SEGMENTS_PARAM = "obstacles.rtree_min_segments"; static constexpr auto RTREE_POINTS_PARAM = "obstacles.rtree_min_points"; - // cspell: ignore OCCUPANCYGRID - enum { POINTCLOUD, OCCUPANCYGRID, STATIC_ONLY } dynamic_source = OCCUPANCYGRID; + enum { POINTCLOUD, OCCUPANCY_GRID, STATIC_ONLY } dynamic_source = OCCUPANCY_GRID; int8_t occupancy_grid_threshold{}; Float dynamic_obstacles_buffer{}; Float dynamic_obstacles_min_vel{}; @@ -74,7 +72,7 @@ struct ObstacleParameters if (type == "pointcloud") { dynamic_source = POINTCLOUD; } else if (type == "occupancy_grid") { - dynamic_source = OCCUPANCYGRID; + dynamic_source = OCCUPANCY_GRID; } else if (type == "static_only") { dynamic_source = STATIC_ONLY; } else { @@ -113,7 +111,7 @@ struct ObstacleParameters struct ProjectionParameters { static constexpr auto MODEL_PARAM = "simulation.model"; - static constexpr auto NBPOINTS_PARAM = "simulation.nb_points"; + static constexpr auto NB_POINTS_PARAM = "simulation.nb_points"; static constexpr auto STEER_OFFSET_PARAM = "simulation.steering_offset"; static constexpr auto DISTANCE_METHOD_PARAM = "simulation.distance_method"; static constexpr auto DURATION_PARAM = "min_ttc"; @@ -135,7 +133,7 @@ struct ProjectionParameters { updateModel(node, node.declare_parameter(MODEL_PARAM)); updateDistanceMethod(node, node.declare_parameter(DISTANCE_METHOD_PARAM)); - updateNbPoints(node, node.declare_parameter(NBPOINTS_PARAM)); + updateNbPoints(node, node.declare_parameter(NB_POINTS_PARAM)); steering_angle_offset = node.declare_parameter(STEER_OFFSET_PARAM); duration = node.declare_parameter(DURATION_PARAM); } diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp index 80d79791d0bbc..9edb197ecba72 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/ros/transform_listener.hpp" #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp index 2ee003206339d..258ef8f01b5f5 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_VELOCITY_LIMITER__TRAJECTORY_PREPROCESSING_HPP_ #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring namespace obstacle_velocity_limiter { diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index 81de7463b9e42..e44011e1f0ee3 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -23,7 +23,6 @@ #include #include -// cspell: ignore multipolygon, multilinestring namespace obstacle_velocity_limiter { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -36,15 +35,15 @@ using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); using point_t = tier4_autoware_utils::Point2d; using multipoint_t = tier4_autoware_utils::MultiPoint2d; using polygon_t = tier4_autoware_utils::Polygon2d; -using multipolygon_t = tier4_autoware_utils::MultiPolygon2d; +using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; using segment_t = tier4_autoware_utils::Segment2d; using linestring_t = tier4_autoware_utils::LineString2d; -using multilinestring_t = tier4_autoware_utils::MultiLineString2d; +using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; struct ObstacleMasks { - polygon_t positive_mask; // discard obstacles outside of this polygon - multipolygon_t negative_masks; // discard obstacles inside of these polygons + polygon_t positive_mask; // discard obstacles outside of this polygon + multi_polygon_t negative_masks; // discard obstacles inside of these polygons }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index d26f6133b0b54..c9255dae679c0 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -22,6 +22,7 @@ from rclpy.node import Node +# cspell: ignore axhline, relim class TrajectoryVisualizer(Node): def __init__(self): super().__init__("trajectory_visualizer") diff --git a/planning/obstacle_velocity_limiter/src/debug.cpp b/planning/obstacle_velocity_limiter/src/debug.cpp index 853dee2d20f49..051846cfeb8ca 100644 --- a/planning/obstacle_velocity_limiter/src/debug.cpp +++ b/planning/obstacle_velocity_limiter/src/debug.cpp @@ -16,7 +16,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -57,8 +56,8 @@ visualization_msgs::msg::Marker makePolygonMarker(const polygon_t & polygon, con } visualization_msgs::msg::MarkerArray makeDebugMarkers( - const Obstacles & obstacles, const std::vector & original_projections, - const std::vector & adjusted_projections, + const Obstacles & obstacles, const std::vector & original_projections, + const std::vector & adjusted_projections, const std::vector & original_footprints, const std::vector & adjusted_footprints, const ObstacleMasks & obstacle_masks, const Float marker_z) diff --git a/planning/obstacle_velocity_limiter/src/forward_projection.cpp b/planning/obstacle_velocity_limiter/src/forward_projection.cpp index d3193eefe0114..e90f17ae140e6 100644 --- a/planning/obstacle_velocity_limiter/src/forward_projection.cpp +++ b/planning/obstacle_velocity_limiter/src/forward_projection.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -48,10 +47,10 @@ segment_t forwardSimulatedSegment( return segment_t{from, to}; } -multilinestring_t bicycleProjectionLines( +multi_linestring_t bicycleProjectionLines( const geometry_msgs::msg::Point & origin, const ProjectionParameters & params) { - multilinestring_t lines; + multi_linestring_t lines; if (params.steering_angle_offset == 0.0) lines.push_back(bicycleProjectionLine(origin, params, params.steering_angle)); else @@ -78,7 +77,7 @@ linestring_t bicycleProjectionLine( return line; } -polygon_t generateFootprint(const multilinestring_t & lines, const double lateral_offset) +polygon_t generateFootprint(const multi_linestring_t & lines, const double lateral_offset) { polygon_t footprint; if (lines.size() == 1) { @@ -98,7 +97,7 @@ polygon_t generateFootprint(const segment_t & segment, const double lateral_offs polygon_t generateFootprint(const linestring_t & linestring, const double lateral_offset) { namespace bg = boost::geometry; - multipolygon_t footprint; + multi_polygon_t footprint; namespace strategy = bg::strategy::buffer; bg::buffer( linestring, footprint, strategy::distance_symmetric(lateral_offset), diff --git a/planning/obstacle_velocity_limiter/src/map_utils.cpp b/planning/obstacle_velocity_limiter/src/map_utils.cpp index e84734355268e..d10244867833b 100644 --- a/planning/obstacle_velocity_limiter/src/map_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/map_utils.cpp @@ -17,7 +17,6 @@ #include "lanelet2_core/primitives/LineString.h" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -27,10 +26,10 @@ namespace obstacle_velocity_limiter { -multilinestring_t extractStaticObstacles( +multi_linestring_t extractStaticObstacles( const lanelet::LaneletMap & lanelet_map, const std::vector & tags) { - multilinestring_t lines; + multi_linestring_t lines; linestring_t line; linestring_t simplified_line; for (const auto & ls : lanelet_map.lineStringLayer) { diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp index dbe7c87922564..ac1b4bde65211 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -34,7 +34,7 @@ Float calculateSafeVelocity( std::max(min_adjusted_velocity, static_cast(dist_to_collision / time_buffer))); } -multipolygon_t createPolygonMasks( +multi_polygon_t createPolygonMasks( const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, const Float min_vel) { @@ -42,7 +42,7 @@ multipolygon_t createPolygonMasks( } std::vector createFootprintPolygons( - const std::vector & projected_linestrings, const Float lateral_offset) + const std::vector & projected_linestrings, const Float lateral_offset) { std::vector footprints; footprints.reserve(projected_linestrings.size()); @@ -84,8 +84,8 @@ polygon_t createEnvelopePolygon( polygon_t createEnvelopePolygon(const std::vector & footprints) { - multipolygon_t unions; - multipolygon_t result; + multi_polygon_t unions; + multi_polygon_t result; for (const auto & footprint : footprints) { boost::geometry::union_(footprint, unions, result); unions = result; @@ -95,10 +95,10 @@ polygon_t createEnvelopePolygon(const std::vector & footprints) return unions.front(); } -std::vector createProjectedLines( +std::vector createProjectedLines( const Trajectory & trajectory, ProjectionParameters & params) { - std::vector projections; + std::vector projections; projections.reserve(trajectory.points.size()); for (const auto & point : trajectory.points) { params.update(point); @@ -114,7 +114,7 @@ std::vector createProjectedLines( void limitVelocity( Trajectory & trajectory, const CollisionChecker & collision_checker, - const std::vector & projections, const std::vector & footprints, + const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params) { Float time = 0.0; diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index a738fff7b0e5f..9ab609378621a 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -21,7 +21,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/trajectory_preprocessing.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -151,7 +150,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParamete result.successful = false; result.reason = "Unknown forward projection model"; } - } else if (parameter.get_name() == ProjectionParameters::NBPOINTS_PARAM) { + } else if (parameter.get_name() == ProjectionParameters::NB_POINTS_PARAM) { if (!projection_params_.updateNbPoints(*this, static_cast(parameter.as_int()))) { result.successful = false; result.reason = "number of points for projections must be at least 2"; diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index 417ff6b7783e2..603e573e68f17 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -58,15 +58,16 @@ polygon_t createObjectPolygon( return translate_obj_poly; } -multipolygon_t createObjectPolygons( +multi_polygon_t createObjectPolygons( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity) { - multipolygon_t polygons; + multi_polygon_t polygons; for (const auto & object : objects.objects) { - if ( - object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_velocity || - object.kinematics.initial_twist_with_covariance.twist.linear.x <= -min_velocity) { + const double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (min_velocity <= obj_vel_norm) { polygons.push_back(createObjectPolygon( object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); } @@ -79,8 +80,7 @@ void addSensorObstacles( const ObstacleMasks & masks, tier4_autoware_utils::TransformListener & transform_listener, const std::string & target_frame, const ObstacleParameters & obstacle_params) { - // cspell: ignore OCCUPANCYGRID - if (obstacle_params.dynamic_source == ObstacleParameters::OCCUPANCYGRID) { + if (obstacle_params.dynamic_source == ObstacleParameters::OCCUPANCY_GRID) { auto grid_map = convertToGridMap(occupancy_grid); threshold(grid_map, obstacle_params.occupancy_grid_threshold); maskPolygons(grid_map, masks); diff --git a/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp index 397e20d05ebe0..7fe3d88885c3a 100644 --- a/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp @@ -15,7 +15,6 @@ #include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -70,7 +69,7 @@ grid_map::GridMap convertToGridMap(const OccupancyGrid & occupancy_grid) return grid_map; } -multilinestring_t extractObstacles( +multi_linestring_t extractObstacles( const grid_map::GridMap & grid_map, const OccupancyGrid & occupancy_grid) { cv::Mat cv_image; @@ -79,7 +78,7 @@ multilinestring_t extractObstacles( cv::erode(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), 2); std::vector> contours; cv::findContours(cv_image, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE); - multilinestring_t obstacles; + multi_linestring_t obstacles; const auto & info = occupancy_grid.info; for (const auto & contour : contours) { linestring_t line; diff --git a/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp index 25339a49529d6..4f9ca6c5bc3e7 100644 --- a/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp @@ -15,7 +15,6 @@ #include "obstacle_velocity_limiter/pointcloud_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index 64693ef8ef249..e9cf8d9ad4dd6 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -15,7 +15,6 @@ #include "obstacle_velocity_limiter/distance.hpp" #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp index 6e35ec95c62e0..536c3e93e4fa7 100644 --- a/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp +++ b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/forward_projection.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -149,10 +148,10 @@ TEST(TestForwardProjection, generateFootprintMultiLinestrings) { using obstacle_velocity_limiter::generateFootprint; using obstacle_velocity_limiter::linestring_t; - using obstacle_velocity_limiter::multilinestring_t; + using obstacle_velocity_limiter::multi_linestring_t; auto footprint = generateFootprint( - multilinestring_t{ + multi_linestring_t{ linestring_t{{0.0, 0.0}, {0.0, 1.0}}, linestring_t{{0.0, 0.0}, {0.8, 0.8}}, linestring_t{{0.0, 0.0}, {1.0, 0.0}}}, 0.5); diff --git a/planning/obstacle_velocity_limiter/test/test_obstacles.cpp b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp index f75e808828ca6..eaf4a29a3a282 100644 --- a/planning/obstacle_velocity_limiter/test/test_obstacles.cpp +++ b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -49,7 +48,7 @@ TEST(TestObstacles, ObstacleTreeLines) /* using obstacle_velocity_limiter::point_t; using obstacle_velocity_limiter::linestring_t; - const obstacle_velocity_limiter::multilinestring_t lines = { + const obstacle_velocity_limiter::multi_linestring_t lines = { {point_t{-0.5, -0.5}, point_t{0.5,0.5}}, {point_t{0, 0}, point_t{1,1}}, {point_t(2, 2), point_t(-2, 2)}, diff --git a/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp index a6ff8543f6ad0..4b49c569ab9f6 100644 --- a/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp +++ b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -22,7 +21,7 @@ TEST(TestOccupancyGridUtils, extractObstacleLines) { - using obstacle_velocity_limiter::multipolygon_t; + using obstacle_velocity_limiter::multi_polygon_t; using obstacle_velocity_limiter::polygon_t; constexpr int8_t occupied_thr = 10; nav_msgs::msg::OccupancyGrid occupancy_grid; @@ -37,7 +36,7 @@ TEST(TestOccupancyGridUtils, extractObstacleLines) constexpr auto extractObstacles = []( const nav_msgs::msg::OccupancyGrid & occupancy_grid, - const multipolygon_t & negative_masks, + const multi_polygon_t & negative_masks, const polygon_t & positive_mask, const double thr) { obstacle_velocity_limiter::ObstacleMasks masks; masks.negative_masks = negative_masks; diff --git a/planning/path_smoother/CMakeLists.txt b/planning/path_smoother/CMakeLists.txt index 30f06f7b87f81..b2fcf9076a7b3 100644 --- a/planning/path_smoother/CMakeLists.txt +++ b/planning/path_smoother/CMakeLists.txt @@ -7,12 +7,7 @@ autoware_package() find_package(Eigen3 REQUIRED) ament_auto_add_library(path_smoother SHARED - # node - src/elastic_band_smoother.cpp - # algorithms - src/elastic_band.cpp - # utils - src/utils/trajectory_utils.cpp + DIRECTORY src ) target_include_directories(path_smoother diff --git a/planning/path_smoother/config/elastic_band_smoother.param.yaml b/planning/path_smoother/config/elastic_band_smoother.param.yaml index 730bc3053e7a3..8e77420dd4ae9 100644 --- a/planning/path_smoother/config/elastic_band_smoother.param.yaml +++ b/planning/path_smoother/config/elastic_band_smoother.param.yaml @@ -35,3 +35,13 @@ # nearest search ego_nearest_dist_threshold: 3.0 # [m] ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + # replanning & trimming trajectory param outside algorithm + replan: + enable: true # if true, only perform smoothing when the input changes significantly + max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_path_shape_forward_lon_dist: 100.0 # forward point to check lateral distance difference [m] + max_path_shape_forward_lat_dist: 0.1 # threshold of path shape change around forward point [m] + max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] + # make max_goal_moving_dist long to keep start point fixed for pull over + max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] + max_delta_time_sec: 1.0 # threshold of delta time for replan [second] diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index 17c6bc6f162af..94f4d62c4a6fd 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -30,15 +30,15 @@ struct Bounds; struct PlannerData { - // input - Header header; - std::vector traj_points; // converted from the input path - std::vector left_bound; - std::vector right_bound; - - // ego + std::vector traj_points; geometry_msgs::msg::Pose ego_pose; double ego_vel{}; + + PlannerData( + std::vector traj_points_, geometry_msgs::msg::Pose ego_pose_, double ego_vel_) + : traj_points(traj_points_), ego_pose(ego_pose_), ego_vel(ego_vel_) + { + } }; struct TimeKeeper diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 26bc3b71d8245..1085ca3815728 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -18,6 +18,7 @@ #include "motion_utils/motion_utils.hpp" #include "path_smoother/common_structs.hpp" #include "path_smoother/elastic_band.hpp" +#include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -59,13 +60,14 @@ class ElasticBandSmoother : public rclcpp::Node // algorithms std::shared_ptr eb_path_smoother_ptr_{nullptr}; + std::shared_ptr replan_checker_ptr_{nullptr}; // parameters CommonParam common_param_{}; EgoNearestParam ego_nearest_param_{}; // variables for subscribers - Odometry::SharedPtr ego_state_ptr_; + Odometry::ConstSharedPtr ego_state_ptr_; // variables for previous information std::shared_ptr> prev_optimized_traj_points_ptr_; @@ -88,7 +90,7 @@ class ElasticBandSmoother : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; // subscriber callback function - void onPath(const Path::SharedPtr); + void onPath(const Path::ConstSharedPtr path_ptr); // reset functions void initializePlanning(); diff --git a/planning/path_smoother/include/path_smoother/replan_checker.hpp b/planning/path_smoother/include/path_smoother/replan_checker.hpp new file mode 100644 index 0000000000000..d06cbc093a0c8 --- /dev/null +++ b/planning/path_smoother/include/path_smoother/replan_checker.hpp @@ -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. + +#ifndef PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#define PATH_SMOOTHER__REPLAN_CHECKER_HPP_ + +#include "path_smoother/common_structs.hpp" +#include "path_smoother/type_alias.hpp" + +#include + +#include +#include + +namespace path_smoother +{ +class ReplanChecker +{ +public: + explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param); + void onParam(const std::vector & parameters); + + bool isResetRequired(const PlannerData & planner_data) const; + + bool isReplanRequired(const PlannerData & planner_data, const rclcpp::Time & current_time) const; + + void updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time); + +private: + EgoNearestParam ego_nearest_param_; + rclcpp::Logger logger_; + + // previous variables for isResetRequired + std::shared_ptr> prev_traj_points_ptr_{nullptr}; + std::shared_ptr prev_ego_pose_ptr_{nullptr}; + + // previous variable for isReplanRequired + std::shared_ptr prev_replanned_time_ptr_{nullptr}; + + // algorithm parameters + bool enable_; + double max_path_shape_around_ego_lat_dist_; + double max_path_shape_forward_lat_dist_; + double max_path_shape_forward_lon_dist_; + double max_ego_moving_dist_; + double max_goal_moving_dist_; + double max_delta_time_sec_; + + bool isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; + bool isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const; +}; +} // namespace path_smoother + +#endif // PATH_SMOOTHER__REPLAN_CHECKER_HPP_ diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index d6434c3935d1e..75bf1cff20857 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -20,8 +20,6 @@ #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 4120b8aba75b8..dbd0f2de92f29 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -71,7 +71,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1)); odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::SharedPtr msg) { ego_state_ptr_ = msg; }); + "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -87,6 +87,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option eb_path_smoother_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, common_param_, time_keeper_ptr_); + replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); // reset planners initializePlanning(); @@ -109,6 +110,7 @@ rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( // parameters for core algorithms eb_path_smoother_ptr_->onParam(parameters); + replan_checker_ptr_->onParam(parameters); // reset planners initializePlanning(); @@ -134,7 +136,7 @@ void ElasticBandSmoother::resetPreviousData() prev_optimized_traj_points_ptr_ = nullptr; } -void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) +void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) { time_keeper_ptr_->init(); time_keeper_ptr_->tic(__func__); @@ -162,11 +164,29 @@ void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr) const auto input_traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); // 1. calculate trajectory with Elastic Band + // 1.a check if replan (= optimization) is required + PlannerData planner_data( + input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + const bool is_replan_required = [&]() { + if (replan_checker_ptr_->isResetRequired(planner_data)) { + // NOTE: always replan when resetting previous optimization + resetPreviousData(); + return true; + } + // check replan when not resetting previous optimization + return !prev_optimized_traj_points_ptr_ || + replan_checker_ptr_->isReplanRequired(planner_data, now()); + }(); + replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); time_keeper_ptr_->tic(__func__); - auto smoothed_traj_points = - eb_path_smoother_ptr_->smoothTrajectory(input_traj_points, ego_state_ptr_->pose.pose); + auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( + input_traj_points, ego_state_ptr_->pose.pose) + : *prev_optimized_traj_points_ptr_; time_keeper_ptr_->toc(__func__, " "); + prev_optimized_traj_points_ptr_ = + std::make_shared>(smoothed_traj_points); + // 2. update velocity applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/path_smoother/src/replan_checker.cpp new file mode 100644 index 0000000000000..95c4e1eb1c002 --- /dev/null +++ b/planning/path_smoother/src/replan_checker.cpp @@ -0,0 +1,210 @@ +// 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 "path_smoother/replan_checker.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "path_smoother/utils/trajectory_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +namespace path_smoother +{ +ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) +: ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) +{ + enable_ = node->declare_parameter("replan.enable"); + max_path_shape_around_ego_lat_dist_ = + node->declare_parameter("replan.max_path_shape_around_ego_lat_dist"); + max_path_shape_forward_lat_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lat_dist"); + max_path_shape_forward_lon_dist_ = + node->declare_parameter("replan.max_path_shape_forward_lon_dist"); + max_ego_moving_dist_ = node->declare_parameter("replan.max_ego_moving_dist"); + max_goal_moving_dist_ = node->declare_parameter("replan.max_goal_moving_dist"); + max_delta_time_sec_ = node->declare_parameter("replan.max_delta_time_sec"); +} + +void ReplanChecker::onParam(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "replan.enable", enable_); + updateParam( + parameters, "replan.max_path_shape_around_ego_lat_dist", max_path_shape_around_ego_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lat_dist", max_path_shape_forward_lat_dist_); + updateParam( + parameters, "replan.max_path_shape_forward_lon_dist", max_path_shape_forward_lon_dist_); + updateParam(parameters, "replan.max_ego_moving_dist", max_ego_moving_dist_); + updateParam(parameters, "replan.max_goal_moving_dist", max_goal_moving_dist_); + updateParam(parameters, "replan.max_delta_time_sec", max_delta_time_sec_); +} + +bool ReplanChecker::isResetRequired(const PlannerData & planner_data) const +{ + const auto & p = planner_data; + + const bool reset_required = [&]() { + // guard for invalid variables + if (!prev_traj_points_ptr_ || !prev_ego_pose_ptr_) { + return true; + } + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // path shape changes + if (isPathAroundEgoChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO( + logger_, "Replan with resetting optimization since path shape around ego changed."); + return true; + } + + // path goal changes + if (isPathGoalChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan with resetting optimization since path goal changed."); + return true; + } + + // ego pose is lost or new ego pose is designated in simulation + const double delta_dist = + tier4_autoware_utils::calcDistance2d(p.ego_pose, prev_ego_pose_ptr_->position); + if (max_ego_moving_dist_ < delta_dist) { + RCLCPP_INFO( + logger_, + "Replan with resetting optimization since current ego pose is far from previous ego pose."); + return true; + } + + return false; + }(); + + return reset_required; +} + +bool ReplanChecker::isReplanRequired( + const PlannerData & planner_data, const rclcpp::Time & current_time) const +{ + if (!enable_) return true; + + // guard for invalid variables + if (!prev_replanned_time_ptr_ || !prev_traj_points_ptr_) return true; + const auto & prev_traj_points = *prev_traj_points_ptr_; + + // time elapses + const double delta_time_sec = (current_time - *prev_replanned_time_ptr_).seconds(); + if (max_delta_time_sec_ < delta_time_sec) return true; + + // path shape changes + if (isPathForwardChanged(planner_data, prev_traj_points)) { + RCLCPP_INFO(logger_, "Replan since path forward shape changed."); + return true; + } + + return false; +} + +void ReplanChecker::updateData( + const PlannerData & planner_data, const bool is_replan_required, + const rclcpp::Time & current_time) +{ + const auto & p = planner_data; + + // update previous information required in this function + prev_traj_points_ptr_ = std::make_shared>(p.traj_points); + prev_ego_pose_ptr_ = std::make_shared(p.ego_pose); + + // update previous information required in this function + if (is_replan_required) { + prev_replanned_time_ptr_ = std::make_shared(current_time); + } +} + +bool ReplanChecker::isPathAroundEgoChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate ego's lateral offset to previous trajectory points + const auto prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + const double prev_ego_lat_offset = + motion_utils::calcLateralOffset(prev_traj_points, p.ego_pose.position, prev_ego_seg_idx); + + // calculate ego's lateral offset to current trajectory points + const auto ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(p.traj_points, p.ego_pose, ego_nearest_param_); + const double ego_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, p.ego_pose.position, ego_seg_idx); + + const double diff_ego_lat_offset = prev_ego_lat_offset - ego_lat_offset; + if (std::abs(diff_ego_lat_offset) < max_path_shape_around_ego_lat_dist_) { + return false; + } + + return true; +} + +bool ReplanChecker::isPathForwardChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // calculate forward point of previous trajectory points + const size_t prev_ego_seg_idx = + trajectory_utils::findEgoSegmentIndex(prev_traj_points, p.ego_pose, ego_nearest_param_); + + // check if distance is larger than the threshold + constexpr double lon_dist_interval = 10.0; + for (double lon_dist = lon_dist_interval; lon_dist <= max_path_shape_forward_lon_dist_; + lon_dist += lon_dist_interval) { + const auto prev_forward_point = + motion_utils::calcLongitudinalOffsetPoint(prev_traj_points, prev_ego_seg_idx, lon_dist); + if (!prev_forward_point) { + continue; + } + + // calculate lateral offset of current trajectory points to prev forward point + const auto forward_seg_idx = + motion_utils::findNearestSegmentIndex(p.traj_points, *prev_forward_point); + const double forward_lat_offset = + motion_utils::calcLateralOffset(p.traj_points, *prev_forward_point, forward_seg_idx); + if (max_path_shape_forward_lat_dist_ < std::abs(forward_lat_offset)) { + return true; + } + } + + return false; +} + +bool ReplanChecker::isPathGoalChanged( + const PlannerData & planner_data, const std::vector & prev_traj_points) const +{ + const auto & p = planner_data; + + // check if the vehicle is stopping + constexpr double min_vel = 1e-3; + if (min_vel < std::abs(p.ego_vel)) { + return false; + } + + const double goal_moving_dist = + tier4_autoware_utils::calcDistance2d(p.traj_points.back(), prev_traj_points.back()); + if (goal_moving_dist < max_goal_moving_dist_) { + return false; + } + + return true; +} +} // namespace path_smoother diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt index c2bee10f972f1..bf7e36c4c0c0a 100644 --- a/planning/planning_debug_tools/CMakeLists.txt +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -50,6 +50,7 @@ ament_auto_package( install(PROGRAMS scripts/trajectory_visualizer.py scripts/closest_velocity_checker.py - scripts/perception_reproducer.py + scripts/perception_replayer/perception_reproducer.py + scripts/perception_replayer/perception_replayer.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/Readme.md index 6d4958783f86d..fe9615614de81 100644 --- a/planning/planning_debug_tools/Readme.md +++ b/planning/planning_debug_tools/Readme.md @@ -188,7 +188,7 @@ PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_sta ## Perception reproducer -This script can overlay the perception results from the rosbag on the planning simulator. +This script can overlay the perception results from the rosbag on the planning simulator synchronized with the simulator's ego pose. In detail, the ego pose in the rosbag which is closest to the current ego pose in the simulator is calculated. The perception results at the timestamp of the closest ego pose is extracted, and published. @@ -211,3 +211,33 @@ ros2 run planning_debug_tools perception_reproducer.py -b ``` Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. + +## Perception replayer + +A part of the feature is under development. + +This script can overlay the perception results from the rosbag on the planning simulator. + +In detail, this script publishes the data at a certain timestamp from the rosbag. +The timestamp will increase according to the real time without any operation. +By using the GUI, you can modify the timestamp by pausing, changing the rate or going back into the past. + +### How to use + +First, launch the planning simulator, and put the ego pose. +Then, run the script according to the following command. + +By designating a rosbag, perception replayer can be launched. +The GUI is launched as well with which a timestamp of rosbag can be managed. + +```bash +ros2 run planning_debug_tools perception_replayer.py -b +``` + +You can designate multiple rosbags in the directory. + +```bash +ros2 run planning_debug_tools perception_replayer.py -b +``` + +Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py new file mode 100755 index 0000000000000..b45357c7bd8b2 --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import copy +import functools +import sys + +from PyQt5.QtWidgets import QApplication +from perception_replayer_common import PerceptionReplayerCommon +import rclpy +from time_manager_widget import TimeManagerWidget +from utils import create_empty_pointcloud +from utils import translate_objects_coordinate + + +class PerceptionReplayer(PerceptionReplayerCommon): + def __init__(self, args): + super().__init__(args, "perception_replayer") + + self.bag_timestamp = self.rosbag_objects_data[0][0] + self.is_pause = False + self.rate = 1.0 + + # initialize widget + self.widget = TimeManagerWidget( + self.rosbag_objects_data[0][0], self.rosbag_objects_data[-1][0] + ) + self.widget.show() + self.widget.button.clicked.connect(self.onPushed) + for button in self.widget.rate_button: + button.clicked.connect(functools.partial(self.onSetRate, button)) + + # start timer callback + self.delta_time = 0.1 + self.timer = self.create_timer(self.delta_time, self.on_timer) + print("Start timer callback") + + def on_timer(self): + timestamp = self.get_clock().now().to_msg() + + self.kill_online_perception_node() + + if self.args.detected_object: + pointcloud_msg = create_empty_pointcloud(timestamp) + self.pointcloud_pub.publish(pointcloud_msg) + + # step timestamp + # get timestamp from slider + self.bag_timestamp = self.rosbag_objects_data[0][ + 0 + ] + self.widget.slider.value() / 1000000 * ( + self.rosbag_objects_data[-1][0] - self.rosbag_objects_data[0][0] + ) + if not self.is_pause: + self.bag_timestamp += self.rate * self.delta_time * 1e9 # seconds to timestamp + # update slider value from the updated timestamp + self.widget.slider.setValue(self.widget.timestamp_to_value(self.bag_timestamp)) + + # extract message by the timestamp + msgs = copy.deepcopy(self.find_topics_by_timestamp(self.bag_timestamp)) + objects_msg = msgs[0] + traffic_signals_msg = msgs[1] + + # objects + if objects_msg: + objects_msg.header.stamp = timestamp + if self.args.detected_object: + if not self.ego_pose: + print("No ego pose found.") + return + + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + log_ego_pose = ego_odom.pose.pose + + translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) + self.objects_pub.publish(objects_msg) + + # traffic signals + if traffic_signals_msg: + traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) + self.prev_traffic_signals_msg = traffic_signals_msg + elif self.prev_traffic_signals_msg: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + + def onPushed(self, event): + if self.widget.button.isChecked(): + self.is_pause = True + else: + self.is_pause = False + + def onSetRate(self, button): + self.rate = float(button.text()) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-b", "--bag", help="rosbag", default=None) + parser.add_argument( + "-d", "--detected-object", help="publish detected object", action="store_true" + ) + parser.add_argument( + "-t", "--tracked-object", help="publish tracked object", action="store_true" + ) + args = parser.parse_args() + + app = QApplication(sys.argv) + + rclpy.init() + node = PerceptionReplayer(args) + + try: + while True: + app.processEvents() + rclpy.spin_once(node, timeout_sec=0.01) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py new file mode 100644 index 0000000000000..a86a9ae9b2bb0 --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from subprocess import CalledProcessError +from subprocess import check_output +import time + +from autoware_auto_perception_msgs.msg import DetectedObjects +from autoware_auto_perception_msgs.msg import PredictedObjects +from autoware_auto_perception_msgs.msg import TrackedObjects +from autoware_auto_perception_msgs.msg import TrafficSignalArray +from nav_msgs.msg import Odometry +import psutil +from rclpy.node import Node +from rclpy.serialization import deserialize_message +from rosbag2_py import StorageFilter +from rosidl_runtime_py.utilities import get_message +from sensor_msgs.msg import PointCloud2 +from utils import open_reader + + +class PerceptionReplayerCommon(Node): + def __init__(self, args, name): + super().__init__(name) + self.args = args + + self.ego_pose = None + self.rosbag_objects_data = [] + self.rosbag_ego_odom_data = [] + self.rosbag_traffic_signals_data = [] + + # subscriber + self.sub_odom = self.create_subscription( + Odometry, "/localization/kinematic_state", self.on_odom, 1 + ) + + # publisher + if self.args.detected_object: + self.objects_pub = self.create_publisher( + DetectedObjects, "/perception/object_recognition/detection/objects", 1 + ) + elif self.args.tracked_object: + self.objects_pub = self.create_publisher( + TrackedObjects, "/perception/object_recognition/tracking/objects", 1 + ) + else: + self.objects_pub = self.create_publisher( + PredictedObjects, "/perception/object_recognition/objects", 1 + ) + + self.pointcloud_pub = self.create_publisher( + PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 + ) + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + + # load rosbag + print("Stared loading rosbag") + if os.path.isdir(args.bag): + for bag_file in sorted(os.listdir(args.bag)): + self.load_rosbag(args.bag + "/" + bag_file) + else: + self.load_rosbag(args.bag) + print("Ended loading rosbag") + + # wait for ready to publish/subscribe + time.sleep(1.0) + + def on_odom(self, odom): + self.ego_pose = odom.pose.pose + + def load_rosbag(self, rosbag2_path: str): + reader = open_reader(str(rosbag2_path)) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + objects_topic = ( + "/perception/object_recognition/detection/objects" + if self.args.detected_object + else "/perception/object_recognition/tracking/objects" + if self.args.tracked_object + else "/perception/object_recognition/objects" + ) + ego_odom_topic = "/localization/kinematic_state" + traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" + topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic]) + reader.set_filter(topic_filter) + + while reader.has_next(): + (topic, data, stamp) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + if topic == objects_topic: + self.rosbag_objects_data.append((stamp, msg)) + if topic == ego_odom_topic: + self.rosbag_ego_odom_data.append((stamp, msg)) + if topic == traffic_signals_topic: + self.rosbag_traffic_signals_data.append((stamp, msg)) + + def kill_online_perception_node(self): + # kill node if required + kill_process_name = None + if self.args.detected_object: + kill_process_name = "dummy_perception_publisher_node" + elif self.args.tracked_object: + kill_process_name = "multi_object_tracker" + else: + kill_process_name = "map_based_prediction" + if kill_process_name: + try: + pid = check_output(["pidof", kill_process_name]) + process = psutil.Process(int(pid[:-1])) + process.terminate() + except CalledProcessError: + pass + + def find_topics_by_timestamp(self, timestamp): + objects_data = None + for data in self.rosbag_objects_data: + if timestamp < data[0]: + objects_data = data[1] + break + + traffic_signals_data = None + for data in self.rosbag_traffic_signals_data: + if timestamp < data[0]: + traffic_signals_data = data[1] + break + + return objects_data, traffic_signals_data + + def find_ego_odom_by_timestamp(self, timestamp): + ego_odom_data = None + for data in self.rosbag_ego_odom_data: + if timestamp < data[0]: + ego_odom_data = data[1] + break + + return ego_odom_data diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py new file mode 100755 index 0000000000000..4228d506d5bec --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import copy + +from perception_replayer_common import PerceptionReplayerCommon +import rclpy +from utils import calc_squared_distance +from utils import create_empty_pointcloud +from utils import translate_objects_coordinate + + +class PerceptionReproducer(PerceptionReplayerCommon): + def __init__(self, args): + super().__init__(args, "perception_reproducer") + + self.ego_pose_idx = None + self.prev_traffic_signals_msg = None + + # start timer callback + self.timer = self.create_timer(0.1, self.on_timer) + print("Start timer callback") + + def on_timer(self): + timestamp = self.get_clock().now().to_msg() + + self.kill_online_perception_node() + + if self.args.detected_object: + pointcloud_msg = create_empty_pointcloud(timestamp) + self.pointcloud_pub.publish(pointcloud_msg) + + if not self.ego_pose: + print("No ego pose found.") + return + + # find nearest ego odom by simulation observation + ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose) + pose_timestamp = ego_odom[0] + log_ego_pose = ego_odom[1].pose.pose + + # extract message by the nearest ego odom timestamp + msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp)) + objects_msg = msgs[0] + traffic_signals_msg = msgs[1] + + # objects + if objects_msg: + objects_msg.header.stamp = timestamp + if self.args.detected_object: + translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) + self.objects_pub.publish(objects_msg) + + # traffic signals + if traffic_signals_msg: + traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) + self.prev_traffic_signals_msg = traffic_signals_msg + elif self.prev_traffic_signals_msg: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + + def find_nearest_ego_odom_by_observation(self, ego_pose): + if self.ego_pose_idx: + start_idx = self.ego_pose_idx - 10 + end_idx = self.ego_pose_idx + 10 + else: + start_idx = 0 + end_idx = len(self.rosbag_ego_odom_data) - 1 + + nearest_idx = 0 + nearest_dist = float("inf") + for idx in range(start_idx, end_idx + 1): + data = self.rosbag_ego_odom_data[idx] + dist = calc_squared_distance(data[1].pose.pose.position, ego_pose.position) + if dist < nearest_dist: + nearest_dist = dist + nearest_idx = idx + + return self.rosbag_ego_odom_data[nearest_idx] + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-b", "--bag", help="rosbag", default=None) + parser.add_argument( + "-d", "--detected-object", help="publish detected object", action="store_true" + ) + parser.add_argument( + "-t", "--tracked-object", help="publish tracked object", action="store_true" + ) + args = parser.parse_args() + + rclpy.init() + node = PerceptionReproducer(args) + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py new file mode 100644 index 0000000000000..382e725b114cd --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from PyQt5 import QtCore +from PyQt5.QtWidgets import QGridLayout +from PyQt5.QtWidgets import QMainWindow +from PyQt5.QtWidgets import QPushButton +from PyQt5.QtWidgets import QSizePolicy +from PyQt5.QtWidgets import QSlider +from PyQt5.QtWidgets import QWidget + + +# With QSlider, the slider's handle cannot be captured if the mouse cursor is not the handle position when pressing the mouse. +class QJumpSlider(QSlider): + def __init__(self, slider_direction, max_value): + super(self.__class__, self).__init__(slider_direction) + + self.max_value = max_value + self.is_mouse_pressed = False + + def mouse_to_value(self, event): + x = event.pos().x() + return int(self.max_value * x / self.width()) + + def mousePressEvent(self, event): + super(self.__class__, self).mousePressEvent(event) + + if event.button() == QtCore.Qt.LeftButton: + self.setValue(self.mouse_to_value(event)) + self.is_mouse_pressed = True + + def mouseMoveEvent(self, event): + super(self.__class__, self).mouseMoveEvent(event) + if self.is_mouse_pressed: + self.setValue(self.mouse_to_value(event)) + + def mouseReleaseEvent(self, event): + super(self.__class__, self).mouseReleaseEvent(event) + + if event.button() == QtCore.Qt.LeftButton: + self.is_mouse_pressed = False + + +class TimeManagerWidget(QMainWindow): + def __init__(self, start_timestamp, end_timestamp): + super(self.__class__, self).__init__() + + self.start_timestamp = start_timestamp + self.end_timestamp = end_timestamp + self.max_value = 1000000 + + self.setupUI() + + def setupUI(self): + self.setObjectName("PerceptionReplayer") + self.resize(480, 120) + self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) + + self.central_widget = QWidget(self) + self.central_widget.setObjectName("central_widget") + + self.grid_layout = QGridLayout(self.central_widget) + self.grid_layout.setContentsMargins(10, 10, 10, 10) + self.grid_layout.setObjectName("grid_layout") + + # rate button + self.rate_button = [] + for i, rate in enumerate([0.1, 0.5, 1.0, 2.0, 5.0, 10.0]): + button = QPushButton(str(rate)) + button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.rate_button.append(button) + self.grid_layout.addWidget(self.rate_button[-1], 0, i, 1, 1) + + # pause button + self.button = QPushButton("pause") + self.button.setCheckable(True) + self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self.grid_layout.addWidget(self.button, 1, 0, 1, -1) + + # slider + self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) + self.slider.setMinimum(0) + self.slider.setMaximum(self.max_value) + self.slider.setValue(0) + self.grid_layout.addWidget(self.slider, 2, 0, 1, -1) + + self.setCentralWidget(self.central_widget) + + def timestamp_to_value(self, timestamp): + return int( + (timestamp - self.start_timestamp) + / (self.end_timestamp - self.start_timestamp) + * self.max_value + ) + + def value_to_timestamp(self, value): + return self.start_timestamp + self.slider.value() / self.max_value * ( + self.end_timestamp - self.start_timestamp + ) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py new file mode 100644 index 0000000000000..572922d4c7abb --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from geometry_msgs.msg import Quaternion +import numpy as np +import rosbag2_py +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from tf_transformations import euler_from_quaternion +from tf_transformations import quaternion_from_euler + + +def get_rosbag_options(path, serialization_format="cdr"): + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format, + ) + + return storage_options, converter_options + + +def open_reader(path: str): + storage_options, converter_options = get_rosbag_options(path) + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + return reader + + +def calc_squared_distance(p1, p2): + return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2) + + +def create_empty_pointcloud(timestamp): + pointcloud_msg = PointCloud2() + pointcloud_msg.header.stamp = timestamp + pointcloud_msg.header.frame_id = "map" + pointcloud_msg.height = 1 + pointcloud_msg.is_dense = True + pointcloud_msg.point_step = 16 + field_name_vec = ["x", "y", "z"] + offset_vec = [0, 4, 8] + for field_name, offset in zip(field_name_vec, offset_vec): + field = PointField() + field.name = field_name + field.offset = offset + field.datatype = 7 + field.count = 1 + pointcloud_msg.fields.append(field) + return pointcloud_msg + + +def get_yaw_from_quaternion(orientation): + orientation_list = [ + orientation.x, + orientation.y, + orientation.z, + orientation.w, + ] + return euler_from_quaternion(orientation_list)[2] + + +def get_quaternion_from_yaw(yaw): + q = quaternion_from_euler(0, 0, yaw) + orientation = Quaternion() + orientation.x = q[0] + orientation.y = q[1] + orientation.z = q[2] + orientation.w = q[3] + return orientation + + +def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg): + log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) + log_ego_pose_trans_mat = np.array( + [ + [ + math.cos(log_ego_yaw), + -math.sin(log_ego_yaw), + log_ego_pose.position.x, + ], + [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + ego_yaw = get_yaw_from_quaternion(ego_pose.orientation) + ego_pose_trans_mat = np.array( + [ + [math.cos(ego_yaw), -math.sin(ego_yaw), ego_pose.position.x], + [math.sin(ego_yaw), math.cos(ego_yaw), ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + for o in objects_msg.objects: + log_object_pose = o.kinematics.pose_with_covariance.pose + log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) + log_object_pos_vec = np.array([log_object_pose.position.x, log_object_pose.position.y, 1.0]) + + # translate object pose from ego pose in log to ego pose in simulation + object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( + log_ego_pose_trans_mat.dot(log_object_pos_vec.T) + ) + + object_pose = o.kinematics.pose_with_covariance.pose + object_pose.position.x = object_pos_vec[0] + object_pose.position.y = object_pos_vec[1] + object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw) diff --git a/planning/planning_debug_tools/scripts/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_reproducer.py deleted file mode 100755 index 5fc367665e256..0000000000000 --- a/planning/planning_debug_tools/scripts/perception_reproducer.py +++ /dev/null @@ -1,338 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import argparse -import copy -import math -import os -from subprocess import CalledProcessError -from subprocess import check_output -import time - -from autoware_auto_perception_msgs.msg import DetectedObjects -from autoware_auto_perception_msgs.msg import PredictedObjects -from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_auto_perception_msgs.msg import TrafficSignalArray -from geometry_msgs.msg import Quaternion -from nav_msgs.msg import Odometry -import numpy as np -import psutil -import rclpy -from rclpy.node import Node -from rclpy.serialization import deserialize_message -import rosbag2_py -from rosbag2_py import StorageFilter -from rosidl_runtime_py.utilities import get_message -from sensor_msgs.msg import PointCloud2 -from sensor_msgs.msg import PointField -from tf_transformations import euler_from_quaternion -from tf_transformations import quaternion_from_euler - - -def get_rosbag_options(path, serialization_format="cdr"): - storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") - - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, - output_serialization_format=serialization_format, - ) - - return storage_options, converter_options - - -def open_reader(path: str): - storage_options, converter_options = get_rosbag_options(path) - reader = rosbag2_py.SequentialReader() - reader.open(storage_options, converter_options) - return reader - - -def calc_squared_distance(p1, p2): - return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2) - - -def create_empty_pointcloud(timestamp): - pointcloud_msg = PointCloud2() - pointcloud_msg.header.stamp = timestamp - pointcloud_msg.header.frame_id = "map" - pointcloud_msg.height = 1 - pointcloud_msg.is_dense = True - pointcloud_msg.point_step = 16 - field_name_vec = ["x", "y", "z"] - offset_vec = [0, 4, 8] - for field_name, offset in zip(field_name_vec, offset_vec): - field = PointField() - field.name = field_name - field.offset = offset - field.datatype = 7 - field.count = 1 - pointcloud_msg.fields.append(field) - return pointcloud_msg - - -def get_yaw_from_quaternion(orientation): - orientation_list = [ - orientation.x, - orientation.y, - orientation.z, - orientation.w, - ] - return euler_from_quaternion(orientation_list)[2] - - -def get_quaternion_from_yaw(yaw): - q = quaternion_from_euler(0, 0, yaw) - orientation = Quaternion() - orientation.x = q[0] - orientation.y = q[1] - orientation.z = q[2] - orientation.w = q[3] - return orientation - - -class PerceptionReproducer(Node): - def __init__(self, args): - super().__init__("perception_reproducer") - self.args = args - - # subscriber - self.sub_odom = self.create_subscription( - Odometry, "/localization/kinematic_state", self.on_odom, 1 - ) - - # publisher - if self.args.detected_object: - self.objects_pub = self.create_publisher( - DetectedObjects, "/perception/object_recognition/detection/objects", 1 - ) - elif self.args.tracked_object: - self.objects_pub = self.create_publisher( - TrackedObjects, "/perception/object_recognition/tracking/objects", 1 - ) - else: - self.objects_pub = self.create_publisher( - PredictedObjects, "/perception/object_recognition/objects", 1 - ) - - self.pointcloud_pub = self.create_publisher( - PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 - ) - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) - - self.ego_pose_idx = None - self.ego_pose = None - - self.prev_traffic_signals_msg = None - - self.rosbag_objects_data = [] - self.rosbag_ego_odom_data = [] - self.rosbag_traffic_signals_data = [] - - # load rosbag - print("Stared loading rosbag") - if os.path.isdir(args.bag): - for bag_file in sorted(os.listdir(args.bag)): - self.load_rosbag(args.bag + "/" + bag_file) - else: - self.load_rosbag(args.bag) - print("Ended loading rosbag") - - # wait for ready to publish/subscribe - time.sleep(1.0) - - self.timer = self.create_timer(0.01, self.on_timer) - print("Start timer callback") - - def on_odom(self, odom): - self.ego_pose = odom.pose.pose - - def on_timer(self): - # kill node if required - kill_process_name = None - if self.args.detected_object: - kill_process_name = "dummy_perception_publisher_node" - elif self.args.tracked_object: - kill_process_name = "multi_object_tracker" - else: - kill_process_name = "map_based_prediction" - if kill_process_name: - try: - pid = check_output(["pidof", kill_process_name]) - process = psutil.Process(int(pid[:-1])) - process.terminate() - except CalledProcessError: - pass - - timestamp = self.get_clock().now().to_msg() - - if self.args.detected_object: - pointcloud_msg = create_empty_pointcloud(timestamp) - self.pointcloud_pub.publish(pointcloud_msg) - - if self.ego_pose: - ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose) - pose_timestamp = ego_odom[0] - log_ego_pose = ego_odom[1].pose.pose - - msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp)) - objects_msg = msgs[0] - traffic_signals_msg = msgs[1] - if objects_msg: - objects_msg.header.stamp = timestamp - if self.args.detected_object: - log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) - log_ego_pose_trans_mat = np.array( - [ - [ - math.cos(log_ego_yaw), - -math.sin(log_ego_yaw), - log_ego_pose.position.x, - ], - [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], - [0.0, 0.0, 1.0], - ] - ) - - ego_yaw = get_yaw_from_quaternion(self.ego_pose.orientation) - ego_pose_trans_mat = np.array( - [ - [math.cos(ego_yaw), -math.sin(ego_yaw), self.ego_pose.position.x], - [math.sin(ego_yaw), math.cos(ego_yaw), self.ego_pose.position.y], - [0.0, 0.0, 1.0], - ] - ) - - for o in objects_msg.objects: - log_object_pose = o.kinematics.pose_with_covariance.pose - log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) - log_object_pos_vec = np.array( - [log_object_pose.position.x, log_object_pose.position.y, 1.0] - ) - - # translate object pose from ego pose in log to ego pose in simulation - object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( - log_ego_pose_trans_mat.dot(log_object_pos_vec.T) - ) - - object_pose = o.kinematics.pose_with_covariance.pose - object_pose.position.x = object_pos_vec[0] - object_pose.position.y = object_pos_vec[1] - object_pose.orientation = get_quaternion_from_yaw( - log_object_yaw + log_ego_yaw - ego_yaw - ) - - self.objects_pub.publish(objects_msg) - if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) - self.prev_traffic_signals_msg = traffic_signals_msg - elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) - else: - print("No ego pose found.") - - def find_nearest_ego_odom_by_observation(self, ego_pose): - if self.ego_pose_idx: - start_idx = self.ego_pose_idx - 10 - end_idx = self.ego_pose_idx + 10 - else: - start_idx = 0 - end_idx = len(self.rosbag_ego_odom_data) - 1 - - nearest_idx = 0 - nearest_dist = float("inf") - for idx in range(start_idx, end_idx + 1): - data = self.rosbag_ego_odom_data[idx] - dist = calc_squared_distance(data[1].pose.pose.position, ego_pose.position) - if dist < nearest_dist: - nearest_dist = dist - nearest_idx = idx - - return self.rosbag_ego_odom_data[nearest_idx] - - def find_topics_by_timestamp(self, timestamp): - objects_data = None - for data in self.rosbag_objects_data: - if timestamp < data[0]: - objects_data = data[1] - break - - traffic_signals_data = None - for data in self.rosbag_traffic_signals_data: - if timestamp < data[0]: - traffic_signals_data = data[1] - break - - return objects_data, traffic_signals_data - - def load_rosbag(self, rosbag2_path: str): - reader = open_reader(str(rosbag2_path)) - - topic_types = reader.get_all_topics_and_types() - # Create a map for quicker lookup - type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} - - objects_topic = ( - "/perception/object_recognition/detection/objects" - if self.args.detected_object - else "/perception/object_recognition/tracking/objects" - if self.args.tracked_object - else "/perception/object_recognition/objects" - ) - ego_odom_topic = "/localization/kinematic_state" - traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" - topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic]) - reader.set_filter(topic_filter) - - while reader.has_next(): - (topic, data, stamp) = reader.read_next() - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - if topic == objects_topic: - self.rosbag_objects_data.append((stamp, msg)) - if topic == ego_odom_topic: - self.rosbag_ego_odom_data.append((stamp, msg)) - if topic == traffic_signals_topic: - self.rosbag_traffic_signals_data.append((stamp, msg)) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("-b", "--bag", help="rosbag", default=None) - parser.add_argument( - "-d", "--detected-object", help="publish detected object", action="store_true" - ) - parser.add_argument( - "-t", "--tracked-object", help="publish tracked object", action="store_true" - ) - args = parser.parse_args() - - rclpy.init() - node = PerceptionReproducer(args) - rclpy.spin(node) - - try: - rclpy.init() - node = PerceptionReproducer(args) - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 9b5775642424c..6c0d2e86943dd 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -38,9 +38,8 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in void calcCurvature(const Trajectory & trajectory, std::vector & curvatures); -// cspell: ignore steerings void calcSteeringAngles( - const Trajectory & trajectory, const double wheelbase, std::vector & steerings); + const Trajectory & trajectory, const double wheelbase, std::vector & steering_array); std::pair calcMaxCurvature(const Trajectory & trajectory); diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index b896b3f6d8979..e4c882cfaedb9 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -216,7 +216,7 @@ std::pair calcMaxRelativeAngles(const Trajectory & trajectory) } void calcSteeringAngles( - const Trajectory & trajectory, const double wheelbase, std::vector & steerings) + const Trajectory & trajectory, const double wheelbase, std::vector & steering_array) { const auto curvatureToSteering = [](const auto k, const auto wheelbase) { return std::atan(k * wheelbase); @@ -225,19 +225,19 @@ void calcSteeringAngles( std::vector curvatures; calcCurvature(trajectory, curvatures); - steerings.clear(); + steering_array.clear(); for (const auto k : curvatures) { - steerings.push_back(curvatureToSteering(k, wheelbase)); + steering_array.push_back(curvatureToSteering(k, wheelbase)); } } std::pair calcMaxSteeringAngles( const Trajectory & trajectory, const double wheelbase) { - std::vector steerings; - calcSteeringAngles(trajectory, wheelbase, steerings); + std::vector steering_array; + calcSteeringAngles(trajectory, wheelbase, steering_array); - return getAbsMaxValAndIdx(steerings); + return getAbsMaxValAndIdx(steering_array); } std::pair calcMaxSteeringRates( @@ -247,8 +247,8 @@ std::pair calcMaxSteeringRates( return {0.0, 0}; } - std::vector steerings; - calcSteeringAngles(trajectory, wheelbase, steerings); + std::vector steering_array; + calcSteeringAngles(trajectory, wheelbase, steering_array); double max_steering_rate = 0.0; size_t max_index = 0; @@ -259,8 +259,8 @@ std::pair calcMaxSteeringRates( const auto v = 0.5 * (p_next.longitudinal_velocity_mps + p_prev.longitudinal_velocity_mps); const auto dt = delta_s / std::max(v, 1.0e-5); - const auto steer_prev = steerings.at(i); - const auto steer_next = steerings.at(i + 1); + const auto steer_prev = steering_array.at(i); + const auto steer_next = steering_array.at(i + 1); const auto steer_rate = (steer_next - steer_prev) / dt; takeBigger(max_steering_rate, max_index, steer_rate, i); diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index c60c0e431108e..1818c460e407b 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -89,13 +89,15 @@ class RouteHandler // for routing bool planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, - lanelet::ConstLanelets * path_lanelets) const; + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes = false) const; std::vector createMapSegments(const lanelet::ConstLanelets & path_lanelets) const; static bool isRouteLooped(const RouteSections & route_sections); // for goal bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const; Pose getGoalPose() const; + Pose getStartPose() const; + Pose getOriginalStartPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -300,6 +302,8 @@ class RouteHandler bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; bool getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const; @@ -372,6 +376,9 @@ class RouteHandler bool is_map_msg_ready_{false}; bool is_handler_ready_{false}; + // save original(not modified) route start pose for start planer execution + Pose original_start_pose_; + // non-const methods void setLaneletsFromRouteMsg(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index fc38d02845796..ef2051d1b244a 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -8,6 +8,7 @@ Zulfaqar Azmi Yutaka Shimizu Kosuke Takeuchi + Takayuki Murooka Apache License 2.0 Fumiya Watanabe diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 3027ca0b96560..b8cedaa947455 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -173,6 +173,10 @@ bool RouteHandler::isRouteLooped(const RouteSections & route_sections) void RouteHandler::setRoute(const LaneletRoute & route_msg) { if (!isRouteLooped(route_msg.segments)) { + // if get not modified route but new route, reset original start pose + if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { + original_start_pose_ = route_msg.start_pose; + } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; setLaneletsFromRouteMsg(); @@ -363,7 +367,7 @@ UUID RouteHandler::getRouteUuid() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getRouteUuid: Route has not been set yet"); - UUID(); + return UUID(); } return route_ptr_->uuid; } @@ -418,6 +422,20 @@ lanelet::ConstLanelets RouteHandler::getRouteLanelets() const return route_lanelets_; } +Pose RouteHandler::getStartPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); + Pose(); + } + return route_ptr_->start_pose; +} + +Pose RouteHandler::getOriginalStartPose() const +{ + return original_start_pose_; +} + Pose RouteHandler::getGoalPose() const { if (!route_ptr_) { @@ -779,6 +797,13 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } +bool RouteHandler::getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet( + preferred_lanelets_, search_pose, closest_lanelet); +} + bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const @@ -917,6 +942,16 @@ bool RouteHandler::isBijectiveConnection( boost::optional RouteHandler::getRightLanelet( const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { + // right road lanelet of shoulder lanelet + if (isShoulderLanelet(lanelet)) { + for (const auto & road_lanelet : road_lanelets_) { + if (lanelet::geometry::rightOf(road_lanelet, lanelet)) { + return road_lanelet; + } + } + return boost::none; + } + // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); if (right_lane) { @@ -934,13 +969,18 @@ boost::optional RouteHandler::getRightLanelet( return adjacent_right_lane; } - lanelet::ConstLanelet next_lanelet; - if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + lanelet::ConstLanelets prev_lanelet; + if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { return adjacent_right_lane; } - lanelet::ConstLanelets prev_lanelet; - if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + if (lanelet.rightBound().back().id() == lane.leftBound().back().id()) { + return lane; + } + } return adjacent_right_lane; } @@ -974,6 +1014,16 @@ bool RouteHandler::getLeftLaneletWithinRoute( boost::optional RouteHandler::getLeftLanelet( const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const { + // left road lanelet of shoulder lanelet + if (isShoulderLanelet(lanelet)) { + for (const auto & road_lanelet : road_lanelets_) { + if (lanelet::geometry::leftOf(road_lanelet, lanelet)) { + return road_lanelet; + } + } + return boost::none; + } + // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); if (left_lane) { @@ -991,13 +1041,18 @@ boost::optional RouteHandler::getLeftLanelet( return adjacent_left_lane; } - lanelet::ConstLanelet next_lanelet; - if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + lanelet::ConstLanelets prev_lanelet; + if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { return adjacent_left_lane; } - lanelet::ConstLanelets prev_lanelet; - if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) { + lanelet::ConstLanelet next_lanelet; + if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) { + for (const auto & lane : getNextLanelets(prev_lanelet.front())) { + if (lanelet.leftBound().back().id() == lane.rightBound().back().id()) { + return lane; + } + } return adjacent_left_lane; } @@ -1974,7 +2029,7 @@ lanelet::ConstLanelets RouteHandler::getNextLaneSequence( bool RouteHandler::planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, - lanelet::ConstLanelets * path_lanelets) const + lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const { // Find lanelets for start point. First, find all lanelets containing the start point to calculate // all possible route later. It fails when the point is not located on any road_lanelet (e.g. the @@ -2038,15 +2093,19 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( } if (is_route_found) { - bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); - if (shortest_path_has_no_drivable_lane) { - drivable_lane_path_found = - findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); - } - lanelet::routing::LaneletPath path; - if (drivable_lane_path_found) { - path = drivable_lane_path; + if (consider_no_drivable_lanes) { + bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); + if (shortest_path_has_no_drivable_lane) { + drivable_lane_path_found = + findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); + } + + if (drivable_lane_path_found) { + path = drivable_lane_path; + } else { + path = shortest_path; + } } else { path = shortest_path; } diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 1661ac9ebef3a..4929d0f8e27f3 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -53,8 +53,8 @@ class RTCInterface const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); void clearCooperateStatus(); - bool isActivated(const UUID & uuid); - bool isRegistered(const UUID & uuid); + bool isActivated(const UUID & uuid) const; + bool isRegistered(const UUID & uuid) const; void lockCommandUpdate(); void unlockCommandUpdate(); @@ -74,10 +74,9 @@ class RTCInterface rclcpp::Publisher::SharedPtr pub_statuses_; rclcpp::Service::SharedPtr srv_commands_; rclcpp::Service::SharedPtr srv_auto_mode_; - - std::mutex mutex_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::Logger logger_; + Module module_; CooperateStatusArray registered_status_; std::vector stored_commands_; @@ -87,6 +86,8 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + + mutable std::mutex mutex_; }; } // namespace rtc_interface diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index f4a95d0acf9d1..dbc82113d403b 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -251,7 +251,7 @@ void RTCInterface::clearCooperateStatus() stored_commands_.clear(); } -bool RTCInterface::isActivated(const UUID & uuid) +bool RTCInterface::isActivated(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( @@ -271,7 +271,7 @@ bool RTCInterface::isActivated(const UUID & uuid) return false; } -bool RTCInterface::isRegistered(const UUID & uuid) +bool RTCInterface::isRegistered(const UUID & uuid) const { std::lock_guard lock(mutex_); const auto itr = std::find_if( diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp index 89d91bd3110f5..93c6ec6b87f36 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp +++ b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp @@ -50,11 +50,11 @@ class Bezier explicit Bezier(const std::vector & control_points); /// @brief return the control points [[nodiscard]] const Eigen::Matrix & getControlPoints() const; - /// @brief return the curve in cartersian frame with the desired resolution + /// @brief return the curve in cartesian frame with the desired resolution [[nodiscard]] std::vector cartesian(const double resolution) const; - /// @brief return the curve in cartersian frame with the desired number of points + /// @brief return the curve in cartesian frame with the desired number of points [[nodiscard]] std::vector cartesian(const int nb_points) const; - /// @brief return the curve in cartersian frame (including angle) with the desired number of + /// @brief return the curve in cartesian frame (including angle) with the desired number of /// points [[nodiscard]] std::vector cartesianWithHeading(const int nb_points) const; /// @brief calculate the curve value for the given parameter t @@ -65,7 +65,7 @@ class Bezier [[nodiscard]] Eigen::Vector2d velocity(const double t) const; /// @brief calculate the acceleration (2nd derivative) for the given parameter t [[nodiscard]] Eigen::Vector2d acceleration(const double t) const; - /// @breif return the heading (in radians) of the tangent for the given parameter t + /// @brief return the heading (in radians) of the tangent for the given parameter t [[nodiscard]] double heading(const double t) const; /// @brief calculate the curvature for the given parameter t [[nodiscard]] double curvature(const double t) const; diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp index 9dea7d34742dc..624cd585b0a72 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp @@ -33,6 +33,7 @@ struct SamplingParameters double mk_max; // Minimum normalized curvature vector magnitude }; /// @brief sample Bezier curves given an initial and final state and sampling parameters +// cspell: ignore Artuñedoet /// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated /// Driving in Urban Environments std::vector sample( diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp index 848d8f11938cf..c44cb5d814d71 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp +++ b/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp @@ -126,15 +126,15 @@ struct Trajectory : sampler_common::Trajectory [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override { - auto * subtraj = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); + auto * sub_trajectory = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); assert(to_idx >= from_idx); - subtraj->reserve(to_idx - from_idx); + sub_trajectory->reserve(to_idx - from_idx); const auto copy_subset = [&](const auto & from, auto & to) { to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); }; - copy_subset(frenet_points, subtraj->frenet_points); - return subtraj; + copy_subset(frenet_points, sub_trajectory->frenet_points); + return sub_trajectory; }; }; diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index ebd628793fa24..3368c49459a55 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -183,17 +183,17 @@ void calculateCartesian( for (const auto & fp : trajectory.frenet_points) trajectory.points.push_back(reference.cartesian(fp)); calculateLengthsAndYaws(trajectory); - std::vector dyaws; - dyaws.reserve(trajectory.yaws.size()); + std::vector d_yaws; + d_yaws.reserve(trajectory.yaws.size()); for (size_t i = 0; i + 1 < trajectory.yaws.size(); ++i) - dyaws.push_back(autoware::common::helper_functions::wrap_angle( + d_yaws.push_back(autoware::common::helper_functions::wrap_angle( trajectory.yaws[i + 1] - trajectory.yaws[i])); - dyaws.push_back(0.0); + d_yaws.push_back(0.0); // Calculate curvatures for (size_t i = 1; i < trajectory.yaws.size(); ++i) { const auto curvature = trajectory.lengths[i] == trajectory.lengths[i - 1] ? 0.0 - : dyaws[i] / (trajectory.lengths[i] - trajectory.lengths[i - 1]); + : d_yaws[i] / (trajectory.lengths[i] - trajectory.lengths[i - 1]); trajectory.curvatures.push_back(curvature); } const auto last_curvature = trajectory.curvatures.empty() ? 0.0 : trajectory.curvatures.back(); @@ -205,7 +205,7 @@ void calculateCartesian( const auto s_acc = trajectory.longitudinal_polynomial->acceleration(time); const auto d_vel = trajectory.lateral_polynomial->velocity(time); const auto d_acc = trajectory.lateral_polynomial->acceleration(time); - Eigen::Rotation2D rotation(dyaws[i]); + Eigen::Rotation2D rotation(d_yaws[i]); Eigen::Vector2d vel_vector{s_vel, d_vel}; Eigen::Vector2d acc_vector{s_acc, d_acc}; const auto vel = rotation * vel_vector; diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index 533c79252dfe4..2d7fe52e5dcad 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -2,7 +2,7 @@ ## Purpose -This package implements a node that uses sampling based planning to generate a drivable trajectry. +This package implements a node that uses sampling based planning to generate a drivable trajectory. ## Feature @@ -33,7 +33,7 @@ Note that the velocity is just taken over from the input path. ## Algorithm -Sampling based planning is decomposed into 3 successives steps: +Sampling based planning is decomposed into 3 successive steps: 1. Sampling: candidate trajectories are generated. 2. Pruning: invalid candidates are discarded. @@ -65,7 +65,7 @@ Each soft constraint is associated with a weight to allow tuning of the preferen ## Limitations The quality of the candidates generated with polynomials in frenet frame greatly depend on the reference path. -If the reference path is not smooth, the resulting candidates will probably be undrivable. +If the reference path is not smooth, the resulting candidates will probably be undriveable. Failure to find a valid trajectory current results in a suddenly stopping trajectory. diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index 32c7cebc39b5f..588f29b06c40d 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -458,7 +458,7 @@ std::vector PathSampler::generatePath(const PlannerData & plann prev_path_ = selected_path; } else { RCLCPP_WARN( - get_logger(), "No valid path found (out of %lu) outputing %s\n", candidate_paths.size(), + get_logger(), "No valid path found (out of %lu) outputting %s\n", candidate_paths.size(), prev_path_ ? "previous path" : "stopping path"); int coll = 0; int da = 0; diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp index b574f87e319fb..4ae6382b82383 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp @@ -68,11 +68,11 @@ std::vector generateBezierPaths( target_state.pose = path_spline.cartesian({target_s, 0}); target_state.curvature = path_spline.curvature(target_s); target_state.heading = path_spline.yaw(target_s); - const auto beziers = + const auto bezier_samples = bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); const auto step = std::min(0.1, params.sampling.resolution / target_length); - for (const auto & bezier : beziers) { + for (const auto & bezier : bezier_samples) { sampler_common::Path path; path.lengths.push_back(0.0); for (double t = 0.0; t <= 1.0; t += step) { @@ -107,19 +107,19 @@ std::vector generateFrenetPaths( // Calculate Velocity and acceleration parametrized over arc length // From appendix I of Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame const auto frenet_yaw = initial_state.heading - path_spline.yaw(s); - const auto path_curv = path_spline.curvature(s); + const auto path_curvature = path_spline.curvature(s); const auto delta_s = 0.001; - initial_frenet_state.lateral_velocity = (1 - path_curv * d) * std::tan(frenet_yaw); - const auto path_curv_deriv = (path_spline.curvature(s + delta_s) - path_curv) / delta_s; + initial_frenet_state.lateral_velocity = (1 - path_curvature * d) * std::tan(frenet_yaw); + const auto path_curvature_deriv = (path_spline.curvature(s + delta_s) - path_curvature) / delta_s; const auto cos_yaw = std::cos(frenet_yaw); if (cos_yaw == 0.0) { initial_frenet_state.lateral_acceleration = 0.0; } else { initial_frenet_state.lateral_acceleration = - -(path_curv_deriv * d + path_curv * initial_frenet_state.lateral_velocity) * + -(path_curvature_deriv * d + path_curvature * initial_frenet_state.lateral_velocity) * std::tan(frenet_yaw) + - ((1 - path_curv * d) / (cos_yaw * cos_yaw)) * - (initial_state.curvature * ((1 - path_curv * d) / cos_yaw) - path_curv); + ((1 - path_curvature * d) / (cos_yaw * cos_yaw)) * + (initial_state.curvature * ((1 - path_curvature * d) / cos_yaw) - path_curvature); } return frenet_planner::generatePaths(path_spline, initial_frenet_state, sampling_parameters); } diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp index 25677a5b2d29b..c17f3479932f3 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp @@ -125,11 +125,11 @@ sampler_common::transform::Spline2D preparePathSpline( control_points.transposeInPlace(); const auto nb_knots = path.size() + spline_dim + 3; Eigen::RowVectorXd knots(nb_knots); - constexpr auto repeat_endknots = 3lu; - const auto knot_step = 1.0 / static_cast(nb_knots - 2 * repeat_endknots); + constexpr auto repeat_end_knots = 3lu; + const auto knot_step = 1.0 / static_cast(nb_knots - 2 * repeat_end_knots); auto i = 0lu; - for (; i < repeat_endknots; ++i) knots[i] = 0.0; - for (; i < nb_knots - repeat_endknots; ++i) knots[i] = knots[i - 1] + knot_step; + for (; i < repeat_end_knots; ++i) knots[i] = 0.0; + for (; i < nb_knots - repeat_end_knots; ++i) knots[i] = knots[i - 1] + knot_step; for (; i < nb_knots; ++i) knots[i] = 1.0; const auto spline = Eigen::Spline(knots, control_points); x.reserve(path.size()); diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp index 54e8c457034a9..1cb14fdf56198 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp +++ b/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp @@ -227,19 +227,19 @@ struct Trajectory : Path [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override { - auto * subtraj = new Trajectory(*Path::subset(from_idx, to_idx)); + auto * sub_trajectory = new Trajectory(*Path::subset(from_idx, to_idx)); const auto copy_subset = [&](const auto & from, auto & to) { to.insert(to.end(), std::next(from.begin(), from_idx), std::next(from.begin(), to_idx)); }; - copy_subset(longitudinal_velocities, subtraj->longitudinal_velocities); - copy_subset(longitudinal_accelerations, subtraj->longitudinal_accelerations); - copy_subset(lateral_velocities, subtraj->lateral_velocities); - copy_subset(lateral_accelerations, subtraj->lateral_accelerations); - copy_subset(jerks, subtraj->jerks); - copy_subset(times, subtraj->times); - return subtraj; + copy_subset(longitudinal_velocities, sub_trajectory->longitudinal_velocities); + copy_subset(longitudinal_accelerations, sub_trajectory->longitudinal_accelerations); + copy_subset(lateral_velocities, sub_trajectory->lateral_velocities); + copy_subset(lateral_accelerations, sub_trajectory->lateral_accelerations); + copy_subset(jerks, sub_trajectory->jerks); + copy_subset(times, sub_trajectory->times); + return sub_trajectory; } [[nodiscard]] Trajectory resample(const double fixed_interval) const diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index dfba038dee898..1d5c7b44dcc45 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -197,8 +197,13 @@ void ScenarioSelectorNode::onMap( void ScenarioSelectorNode::onRoute( const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg) { + // When the route id is the same (e.g. rerouting with modified goal) keep the current scenario. + // Otherwise, reset the scenario. + if (!route_handler_ || route_handler_->getRouteUuid() != msg->uuid) { + current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; + } + route_ = msg; - current_scenario_ = tier4_planning_msgs::msg::Scenario::EMPTY; } void ScenarioSelectorNode::onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg) diff --git a/sensing/geo_pos_conv/CHANGELOG.rst b/sensing/geo_pos_conv/CHANGELOG.rst deleted file mode 100644 index 12d311b5aaa5d..0000000000000 --- a/sensing/geo_pos_conv/CHANGELOG.rst +++ /dev/null @@ -1,203 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package geo_pos_conv -^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.0 (2020-10-03) ------------------- -* Convert package to ROS 2 - -1.11.0 (2019-03-21) -------------------- -* add constructor (`#1913 `_) -* Fix license notice in corresponding package.xml -* Contributors: YamatoAndo, amc-nu - -1.10.0 (2019-01-17) -------------------- -* Switch to Apache 2 license (develop branch) (`#1741 `_) - * Switch to Apache 2 - * Replace BSD-3 license header with Apache 2 and reassign copyright to the - Autoware Foundation. - * Update license on Python files - * Update copyright years - * Add #ifndef/define _POINTS_IMAGE_H\_ - * Updated license comment -* Contributors: Esteve Fernandez - -1.9.1 (2018-11-06) ------------------- - -1.9.0 (2018-10-31) ------------------- - -1.8.0 (2018-08-31) ------------------- - -1.7.0 (2018-05-18) ------------------- -* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst -* [fix] Fixes for all packages and dependencies (`#1240 `_) - * Initial Cleanup - * fixed also for indigo - * kf cjeck - * Fix road wizard - * Added travis ci - * Trigger CI - * Fixes to cv_tracker and lidar_tracker cmake - * Fix kitti player dependencies - * Removed unnecessary dependencies - * messages fixing for can - * Update build script travis - * Travis Path - * Travis Paths fix - * Travis test - * Eigen checks - * removed unnecessary dependencies - * Eigen Detection - * Job number reduced - * Eigen3 more fixes - * More Eigen3 - * Even more Eigen - * find package cmake modules included - * More fixes to cmake modules - * Removed non ros dependency - * Enable industrial_ci for indidog and kinetic - * Wrong install command - * fix rviz_plugin install - * FastVirtualScan fix - * Fix Qt5 Fastvirtualscan - * Fixed qt5 system dependencies for rosdep - * NDT TKU Fix catkin not pacakged - * More in detail dependencies fixes for more packages - * GLEW library for ORB - * Ignore OrbLocalizer - * Ignore Version checker - * Fix for driveworks interface - * driveworks not catkinpackagedd - * Missing catkin for driveworks - * libdpm opencv not catkin packaged - * catkin lib gnss not included in obj_db - * Points2Polygon fix - * More missing dependencies - * image viewer not packaged - * Fixed SSH2 detection, added viewers for all distros - * Fix gnss localizer incorrect dependency config - * Fixes to multiple packages dependencies - * gnss plib and package - * More fixes to gnss - * gnss dependencies for gnss_loclaizer - * Missing gnss dependency for gnss on localizer - * More fixes for dependencies - Replaced gnss for autoware_gnss_library - * gnss more fixes - * fixes to more dependencies - * header dependency - * Debug message - * more debug messages changed back to gnss - * debud messages - * gnss test - * gnss install command - * Several fixes for OpenPlanner and its lbiraries - * Fixes to ROSInterface - * More fixes to robotsdk and rosinterface - * robotsdk calibration fix - * Fixes to rosinterface robotsdk libraries and its nodes - * Fixes to Qt5 missing dependencies in robotsdk - * glviewer missing dependencies - * Missing qt specific config cmake for robotsdk - * disable cv_tracker - * Fix to open planner un needed dependendecies - * Fixes for libraries indecision maker - * Fixes to libraries decision_maker installation - * Gazebo on Kinetic - * Added Missing library - * * Removed Gazebo and synchonization packages - * Renames vmap in lane_planner - * Added installation commands for missing pakcages - * Fixes to lane_planner - * Added NDT TKU Glut extra dependencies - * ndt localizer/lib fast pcl fixes - re enable cv_tracker - * Fix kf_lib - * Keep industrial_ci - * Fixes for dpm library - * Fusion lib fixed - * dpm and fusion header should match exported project name - * Fixes to dpm_ocv ndt_localizer and pcl_omp - * no fast_pcl anymore - * fixes to libdpm and its package - * CI test - * test with native travis ci - * missing update for apt - * Fixes to pcl_omp installation and headers - * Final fixes for tests, modified README - * * Fixes to README - * Enable industrial_ci - * re enable native travis tests -* Contributors: Abraham Monrroy, Kosuke Murakami - -1.6.3 (2018-03-06) ------------------- - -1.6.2 (2018-02-27) ------------------- -* Update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.1 (2018-01-20) ------------------- -* update CHANGELOG -* Contributors: Yusuke FUJII - -1.6.0 (2017-12-11) ------------------- -* Prepare release for 1.6.0 -* support all plane(1-19) in geo_pos_conv -* Contributors: Yamato ANDO, yukikitsukawa - -1.5.1 (2017-09-25) ------------------- -* Release/1.5.1 (`#816 `_) - * fix a build error by gcc version - * fix build error for older indigo version - * update changelog for v1.5.1 - * 1.5.1 -* Contributors: Yusuke FUJII - -1.5.0 (2017-09-21) ------------------- -* Update changelog -* install target -* Contributors: Dejan Pangercic, Yusuke FUJII - -1.4.0 (2017-08-04) ------------------- -* version number must equal current release number so we can start releasing in the future -* added changelogs -* Contributors: Dejan Pangercic - -1.3.1 (2017-07-16) ------------------- - -1.3.0 (2017-07-14) ------------------- - -1.2.0 (2017-06-07) ------------------- - -1.1.2 (2017-02-27 23:10) ------------------------- - -1.1.1 (2017-02-27 22:25) ------------------------- - -1.1.0 (2017-02-24) ------------------- - -1.0.1 (2017-01-14) ------------------- - -1.0.0 (2016-12-22) ------------------- -* Initial commit for public release -* Contributors: Shinpei Kato diff --git a/sensing/geo_pos_conv/CMakeLists.txt b/sensing/geo_pos_conv/CMakeLists.txt deleted file mode 100644 index 3c3fad0fadf5c..0000000000000 --- a/sensing/geo_pos_conv/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(geo_pos_conv) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(geo_pos_conv SHARED - src/geo_pos_conv.cpp -) - -ament_auto_package() diff --git a/sensing/geo_pos_conv/README.md b/sensing/geo_pos_conv/README.md deleted file mode 100644 index c75a642bdefb5..0000000000000 --- a/sensing/geo_pos_conv/README.md +++ /dev/null @@ -1,21 +0,0 @@ -# geo_pos_conv - -## Purpose - -The `geo_pos_conv` is a library to calculate the conversion between **x, y positions on the plane rectangular coordinate** and **latitude/longitude on the earth**. - -## Inner-workings / Algorithms - -## Inputs / Outputs - -## Parameters - -## Assumptions / Known limits - -## (Optional) Error detection and handling - -## (Optional) Performance characterization - -## (Optional) References/External links - -## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp b/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp deleted file mode 100644 index 68c4b408efd2d..0000000000000 --- a/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp +++ /dev/null @@ -1,56 +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 GEO_POS_CONV__GEO_POS_CONV_HPP_ -#define GEO_POS_CONV__GEO_POS_CONV_HPP_ - -#include - -class geo_pos_conv -{ -private: - double m_x; // m - double m_y; // m - double m_z; // m - - double m_lat; // latitude - double m_lon; // longitude - double m_h; - - double m_PLato; // plane lat - double m_PLo; // plane lon - -public: - geo_pos_conv(); - double x() const; - double y() const; - double z() const; - - void set_plane(double lat, double lon); - void set_plane(int num); - void set_xyz(double cx, double cy, double cz); - - // set llh in radians - void set_llh(double lat, double lon, double h); - - // set llh in nmea degrees - void set_llh_nmea_degrees(double latd, double lond, double h); - - void llh_to_xyz(double lat, double lon, double ele); - - void conv_llh2xyz(void); - void conv_xyz2llh(void); -}; - -#endif // GEO_POS_CONV__GEO_POS_CONV_HPP_ diff --git a/sensing/geo_pos_conv/src/geo_pos_conv.cpp b/sensing/geo_pos_conv/src/geo_pos_conv.cpp deleted file mode 100644 index 5fc489d46980f..0000000000000 --- a/sensing/geo_pos_conv/src/geo_pos_conv.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/* - * Copyright 2015-2019 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. - */ - -#include "geo_pos_conv/geo_pos_conv.hpp" - -geo_pos_conv::geo_pos_conv() -: m_x(0), m_y(0), m_z(0), m_lat(0), m_lon(0), m_h(0), m_PLato(0), m_PLo(0) -{ -} - -double geo_pos_conv::x() const -{ - return m_x; -} - -double geo_pos_conv::y() const -{ - return m_y; -} - -double geo_pos_conv::z() const -{ - return m_z; -} - -void geo_pos_conv::set_plane(double lat, double lon) -{ - m_PLato = lat; - m_PLo = lon; -} - -void geo_pos_conv::set_plane(int num) -{ - int lon_deg, lon_min, lat_deg, - lat_min; // longitude and latitude of origin of each plane in Japan - if (num == 1) { - lon_deg = 33; - lon_min = 0; - lat_deg = 129; - lat_min = 30; - } else if (num == 2) { - lon_deg = 33; - lon_min = 0; - lat_deg = 131; - lat_min = 0; - } else if (num == 3) { - lon_deg = 36; - lon_min = 0; - lat_deg = 132; - lat_min = 10; - } else if (num == 4) { - lon_deg = 33; - lon_min = 0; - lat_deg = 133; - lat_min = 30; - } else if (num == 5) { - lon_deg = 36; - lon_min = 0; - lat_deg = 134; - lat_min = 20; - } else if (num == 6) { - lon_deg = 36; - lon_min = 0; - lat_deg = 136; - lat_min = 0; - } else if (num == 7) { - lon_deg = 36; - lon_min = 0; - lat_deg = 137; - lat_min = 10; - } else if (num == 8) { - lon_deg = 36; - lon_min = 0; - lat_deg = 138; - lat_min = 30; - } else if (num == 9) { - lon_deg = 36; - lon_min = 0; - lat_deg = 139; - lat_min = 50; - } else if (num == 10) { - lon_deg = 40; - lon_min = 0; - lat_deg = 140; - lat_min = 50; - } else if (num == 11) { - lon_deg = 44; - lon_min = 0; - lat_deg = 140; - lat_min = 15; - } else if (num == 12) { - lon_deg = 44; - lon_min = 0; - lat_deg = 142; - lat_min = 15; - } else if (num == 13) { - lon_deg = 44; - lon_min = 0; - lat_deg = 144; - lat_min = 15; - } else if (num == 14) { - lon_deg = 26; - lon_min = 0; - lat_deg = 142; - lat_min = 0; - } else if (num == 15) { - lon_deg = 26; - lon_min = 0; - lat_deg = 127; - lat_min = 30; - } else if (num == 16) { - lon_deg = 26; - lon_min = 0; - lat_deg = 124; - lat_min = 0; - } else if (num == 17) { - lon_deg = 26; - lon_min = 0; - lat_deg = 131; - lat_min = 0; - } else if (num == 18) { - lon_deg = 20; - lon_min = 0; - lat_deg = 136; - lat_min = 0; - } else if (num == 19) { - lon_deg = 26; - lon_min = 0; - lat_deg = 154; - lat_min = 0; - } else { - lon_deg = 0; - lon_min = 0; - lat_deg = 0; - lat_min = 0; - } - - // swap longitude and latitude - m_PLo = M_PI * (lat_deg + lat_min / 60.0) / 180.0; - m_PLato = M_PI * (lon_deg + lon_min / 60.0) / 180; -} - -void geo_pos_conv::set_xyz(double cx, double cy, double cz) -{ - m_x = cx; - m_y = cy; - m_z = cz; - conv_xyz2llh(); -} - -void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h) -{ - double lat, lad, lod, lon; - // 1234.56 -> 12'34.56 -> 12+ 34.56/60 - - lad = floor(latd / 100.); - lat = latd - lad * 100.; - lod = floor(lond / 100.); - lon = lond - lod * 100.; - - // Changing Longitude and Latitude to Radians - m_lat = (lad + lat / 60.0) * M_PI / 180; - m_lon = (lod + lon / 60.0) * M_PI / 180; - m_h = h; - - conv_llh2xyz(); -} - -void geo_pos_conv::llh_to_xyz(double lat, double lon, double ele) -{ - m_lat = lat * M_PI / 180; - m_lon = lon * M_PI / 180; - m_h = ele; - - conv_llh2xyz(); -} - -void geo_pos_conv::conv_llh2xyz(void) -{ - double PS; // - double PSo; // - double PDL; // - double Pt; // - double PN; // - double PW; // - - double PB1, PB2, PB3, PB4, PB5, PB6, PB7, PB8, PB9; - double PA, PB, PC, PD, PE, PF, PG, PH, PI; - double Pe; // - double Pet; // - double Pnn; // - double AW, FW, Pmo; - - Pmo = 0.9999; - - /*WGS84 Parameters*/ - AW = 6378137.0; // Semi-major Axis - FW = 1.0 / 298.257222101; // 298.257223563 //Geometrical flattening - - Pe = sqrt(2.0 * FW - pow(FW, 2)); - Pet = sqrt(pow(Pe, 2) / (1.0 - pow(Pe, 2))); - - PA = 1.0 + 3.0 / 4.0 * pow(Pe, 2) + 45.0 / 64.0 * pow(Pe, 4) + 175.0 / 256.0 * pow(Pe, 6) + - 11025.0 / 16384.0 * pow(Pe, 8) + 43659.0 / 65536.0 * pow(Pe, 10) + - 693693.0 / 1048576.0 * pow(Pe, 12) + 19324305.0 / 29360128.0 * pow(Pe, 14) + - 4927697775.0 / 7516192768.0 * pow(Pe, 16); - - PB = 3.0 / 4.0 * pow(Pe, 2) + 15.0 / 16.0 * pow(Pe, 4) + 525.0 / 512.0 * pow(Pe, 6) + - 2205.0 / 2048.0 * pow(Pe, 8) + 72765.0 / 65536.0 * pow(Pe, 10) + - 297297.0 / 262144.0 * pow(Pe, 12) + 135270135.0 / 117440512.0 * pow(Pe, 14) + - 547521975.0 / 469762048.0 * pow(Pe, 16); - - PC = 15.0 / 64.0 * pow(Pe, 4) + 105.0 / 256.0 * pow(Pe, 6) + 2205.0 / 4096.0 * pow(Pe, 8) + - 10395.0 / 16384.0 * pow(Pe, 10) + 1486485.0 / 2097152.0 * pow(Pe, 12) + - 45090045.0 / 58720256.0 * pow(Pe, 14) + 766530765.0 / 939524096.0 * pow(Pe, 16); - - PD = 35.0 / 512.0 * pow(Pe, 6) + 315.0 / 2048.0 * pow(Pe, 8) + 31185.0 / 131072.0 * pow(Pe, 10) + - 165165.0 / 524288.0 * pow(Pe, 12) + 45090045.0 / 117440512.0 * pow(Pe, 14) + - 209053845.0 / 469762048.0 * pow(Pe, 16); - - PE = 315.0 / 16384.0 * pow(Pe, 8) + 3465.0 / 65536.0 * pow(Pe, 10) + - 99099.0 / 1048576.0 * pow(Pe, 12) + 4099095.0 / 29360128.0 * pow(Pe, 14) + - 348423075.0 / 1879048192.0 * pow(Pe, 16); - - PF = 693.0 / 131072.0 * pow(Pe, 10) + 9009.0 / 524288.0 * pow(Pe, 12) + - 4099095.0 / 117440512.0 * pow(Pe, 14) + 26801775.0 / 469762048.0 * pow(Pe, 16); - - PG = 3003.0 / 2097152.0 * pow(Pe, 12) + 315315.0 / 58720256.0 * pow(Pe, 14) + - 11486475.0 / 939524096.0 * pow(Pe, 16); - - PH = 45045.0 / 117440512.0 * pow(Pe, 14) + 765765.0 / 469762048.0 * pow(Pe, 16); - - PI = 765765.0 / 7516192768.0 * pow(Pe, 16); - - PB1 = AW * (1.0 - pow(Pe, 2)) * PA; - PB2 = AW * (1.0 - pow(Pe, 2)) * PB / -2.0; - PB3 = AW * (1.0 - pow(Pe, 2)) * PC / 4.0; - PB4 = AW * (1.0 - pow(Pe, 2)) * PD / -6.0; - PB5 = AW * (1.0 - pow(Pe, 2)) * PE / 8.0; - PB6 = AW * (1.0 - pow(Pe, 2)) * PF / -10.0; - PB7 = AW * (1.0 - pow(Pe, 2)) * PG / 12.0; - PB8 = AW * (1.0 - pow(Pe, 2)) * PH / -14.0; - PB9 = AW * (1.0 - pow(Pe, 2)) * PI / 16.0; - - PS = PB1 * m_lat + PB2 * sin(2.0 * m_lat) + PB3 * sin(4.0 * m_lat) + PB4 * sin(6.0 * m_lat) + - PB5 * sin(8.0 * m_lat) + PB6 * sin(10.0 * m_lat) + PB7 * sin(12.0 * m_lat) + - PB8 * sin(14.0 * m_lat) + PB9 * sin(16.0 * m_lat); - - PSo = PB1 * m_PLato + PB2 * sin(2.0 * m_PLato) + PB3 * sin(4.0 * m_PLato) + - PB4 * sin(6.0 * m_PLato) + PB5 * sin(8.0 * m_PLato) + PB6 * sin(10.0 * m_PLato) + - PB7 * sin(12.0 * m_PLato) + PB8 * sin(14.0 * m_PLato) + PB9 * sin(16.0 * m_PLato); - - PDL = m_lon - m_PLo; - Pt = tan(m_lat); - PW = sqrt(1.0 - pow(Pe, 2) * pow(sin(m_lat), 2)); - PN = AW / PW; - Pnn = sqrt(pow(Pet, 2) * pow(cos(m_lat), 2)); - - m_x = ((PS - PSo) + (1.0 / 2.0) * PN * pow(cos(m_lat), 2.0) * Pt * pow(PDL, 2.0) + - (1.0 / 24.0) * PN * pow(cos(m_lat), 4) * Pt * - (5.0 - pow(Pt, 2) + 9.0 * pow(Pnn, 2) + 4.0 * pow(Pnn, 4)) * pow(PDL, 4) - - (1.0 / 720.0) * PN * pow(cos(m_lat), 6) * Pt * - (-61.0 + 58.0 * pow(Pt, 2) - pow(Pt, 4) - 270.0 * pow(Pnn, 2) + - 330.0 * pow(Pt, 2) * pow(Pnn, 2)) * - pow(PDL, 6) - - (1.0 / 40320.0) * PN * pow(cos(m_lat), 8) * Pt * - (-1385.0 + 3111 * pow(Pt, 2) - 543 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 8)) * - Pmo; - - m_y = (PN * cos(m_lat) * PDL - - 1.0 / 6.0 * PN * pow(cos(m_lat), 3) * (-1 + pow(Pt, 2) - pow(Pnn, 2)) * pow(PDL, 3) - - 1.0 / 120.0 * PN * pow(cos(m_lat), 5) * - (-5.0 + 18.0 * pow(Pt, 2) - pow(Pt, 4) - 14.0 * pow(Pnn, 2) + - 58.0 * pow(Pt, 2) * pow(Pnn, 2)) * - pow(PDL, 5) - - 1.0 / 5040.0 * PN * pow(cos(m_lat), 7) * - (-61.0 + 479.0 * pow(Pt, 2) - 179.0 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 7)) * - Pmo; - - m_z = m_h; -} - -void geo_pos_conv::conv_xyz2llh(void) -{ - // n/a -} diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index d37152297c61d..6eef2cc243ef5 100644 --- a/sensing/gnss_poser/README.md +++ b/sensing/gnss_poser/README.md @@ -27,15 +27,14 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------------------------- | -| `base_frame` | string | "base_link" | frame id | -| `gnss_frame` | string | "gnss" | frame id | -| `gnss_base_frame` | string | "gnss_base_link" | frame id | -| `map_frame` | string | "map" | frame id | -| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | -| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. | -| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | +| Name | Type | Default Value | Description | +| ---------------------- | ------ | ---------------- | -------------------------------------------------------------------------------------------------------- | +| `base_frame` | string | "base_link" | frame id | +| `gnss_frame` | string | "gnss" | frame id | +| `gnss_base_frame` | string | "gnss_base_link" | frame id | +| `map_frame` | string | "map" | frame id | +| `coordinate_system` | int | "4" | coordinate system enumeration; 1: MGRS, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System | +| `gnss_pose_pub_method` | int | 0 | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. | ## Assumptions / Known limits diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 3e4122893ecb6..5c1f621626aa2 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include @@ -62,7 +61,6 @@ GNSSStat NavSatFix2LocalCartesianWGS84( sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) { GNSSStat local_cartesian; - local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN_WGS84; try { GeographicLib::LocalCartesian localCartesian_origin( @@ -84,7 +82,6 @@ GNSSStat NavSatFix2UTM( int height_system) { GNSSStat utm; - utm.coordinate_system = CoordinateSystem::UTM; try { GeographicLib::UTMUPS::Forward( @@ -108,11 +105,9 @@ GNSSStat NavSatFix2LocalCartesianUTM( sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) { GNSSStat utm_local; - utm_local.coordinate_system = CoordinateSystem::UTM; try { // origin of the local coordinate system in global frame GNSSStat utm_origin; - utm_origin.coordinate_system = CoordinateSystem::UTM; GeographicLib::UTMUPS::Forward( nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, utm_origin.east_north_up, utm_origin.x, utm_origin.y); @@ -151,7 +146,6 @@ GNSSStat UTM2MGRS( constexpr int GZD_ID_size = 5; // size of header like "53SPU" GNSSStat mgrs = utm; - mgrs.coordinate_system = CoordinateSystem::MGRS; try { std::string mgrs_code; GeographicLib::MGRS::Forward( @@ -183,20 +177,6 @@ GNSSStat NavSatFix2MGRS( return mgrs; } -GNSSStat NavSatFix2PLANE( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const int & plane_zone, - const rclcpp::Logger & logger) -{ - GNSSStat plane; - plane.coordinate_system = CoordinateSystem::PLANE; - geo_pos_conv geo; - geo.set_plane(plane_zone); - geo.llh_to_xyz(nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude); - plane.x = geo.y(); - plane.y = geo.x(); - plane.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); - return plane; -} } // namespace gnss_poser #endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 2b729a9e374f7..4baaec2dfe80e 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -100,8 +100,6 @@ class GNSSPoser : public rclcpp::Node boost::circular_buffer position_buffer_; - int plane_zone_; - autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr msg_gnss_ins_orientation_stamped_; int height_system_; diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index 46fdc6eeff9ee..fa17bf0e6e232 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -18,19 +18,12 @@ namespace gnss_poser { -enum class CoordinateSystem { - UTM = 0, - MGRS = 1, - PLANE = 2, - LOCAL_CARTESIAN_WGS84 = 3, - LOCAL_CARTESIAN_UTM = 4 -}; +enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_WGS84 = 3, LOCAL_CARTESIAN_UTM = 4 }; struct GNSSStat { GNSSStat() - : coordinate_system(CoordinateSystem::MGRS), - east_north_up(true), + : east_north_up(true), zone(0), mgrs_zone(""), x(0), @@ -42,7 +35,6 @@ struct GNSSStat { } - CoordinateSystem coordinate_system; bool east_north_up; int zone; std::string mgrs_zone; diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index e7dc06864ce84..3883a492358d6 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -13,10 +13,9 @@ - + - @@ -34,7 +33,6 @@ - diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 4887494614a81..1488f329edecb 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -14,7 +14,6 @@ libboost-dev autoware_sensing_msgs - geo_pos_conv geographiclib geometry_msgs rclcpp diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 8fc7d9c1599f5..7d244ce188eee 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -32,7 +32,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) gnss_base_frame_(declare_parameter("gnss_base_frame", "gnss_base_link")), map_frame_(declare_parameter("map_frame", "map")), use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), - plane_zone_(declare_parameter("plane_zone", 9)), msg_gnss_ins_orientation_stamped_( std::make_shared()), height_system_(declare_parameter("height_system", 1)), @@ -191,16 +190,12 @@ GNSSStat GNSSPoser::convert( int height_system) { GNSSStat gnss_stat; - if (coordinate_system == CoordinateSystem::UTM) { - gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { + if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) { gnss_stat = NavSatFix2LocalCartesianUTM( nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system); } else if (coordinate_system == CoordinateSystem::MGRS) { gnss_stat = NavSatFix2MGRS( nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system); - } else if (coordinate_system == CoordinateSystem::PLANE) { - gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger()); } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) { gnss_stat = NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); diff --git a/sensing/image_diagnostics/image/block_state_decision.svg b/sensing/image_diagnostics/image/block_state_decision.svg index 373dc7984b3a6..39c49ab325dda 100644 --- a/sensing/image_diagnostics/image/block_state_decision.svg +++ b/sensing/image_diagnostics/image/block_state_decision.svg @@ -1,5 +1,5 @@ - + @@ -30,8 +29,8 @@ - - + + @@ -165,7 +164,7 @@ blockage area ratio - + @@ -187,7 +186,7 @@ freq_average  <... - + @@ -434,7 +433,7 @@ - + Text is not SVG - cannot display diff --git a/sensing/image_diagnostics/image/image_diagnostics_overview.svg b/sensing/image_diagnostics/image/image_diagnostics_overview.svg index ed72197c7118e..7634974501746 100644 --- a/sensing/image_diagnostics/image/image_diagnostics_overview.svg +++ b/sensing/image_diagnostics/image/image_diagnostics_overview.svg @@ -1,5 +1,5 @@ - + - - + + @@ -111,7 +110,7 @@ - + Text is not SVG - cannot display diff --git a/sensing/image_diagnostics/image/image_status_decision.svg b/sensing/image_diagnostics/image/image_status_decision.svg index ec5ab743e7900..119b2dfe79e24 100644 --- a/sensing/image_diagnostics/image/image_status_decision.svg +++ b/sensing/image_diagnostics/image/image_status_decision.svg @@ -1,15 +1,14 @@ - + @@ -32,8 +31,8 @@ - - + + @@ -49,10 +48,10 @@ Dark blocks > N_dark_... - - - - + + + + @@ -71,10 +70,10 @@ backlight blocks... - - - - + + + + @@ -86,16 +85,16 @@
bloackage blocks > N_blockage_thresh
+ >blockage blocks > N_blockage_thresh
- bloackage blocks > N_b... + blockage blocks > N_bl... - + - + @@ -365,7 +364,7 @@ - + Text is not SVG - cannot display diff --git a/sensing/image_diagnostics/src/image_diagnostics_node.cpp b/sensing/image_diagnostics/src/image_diagnostics_node.cpp index bd5fbf791261e..3db9ab94f50fa 100644 --- a/sensing/image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/image_diagnostics/src/image_diagnostics_node.cpp @@ -42,10 +42,10 @@ ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) void ImageDiagNode::onImageDiagChecker(DiagnosticStatusWrapper & stat) { - stat.add("number dark blocks ", std::to_string(params_.num_of_regions_dark)); - stat.add("number blockaged blocks ", std::to_string(params_.num_of_regions_blockage)); - stat.add("number low visibility blocks ", std::to_string(params_.num_of_regions_low_visibility)); - stat.add("number backlight blocks ", std::to_string(params_.num_of_regions_backlight)); + stat.add("number dark regions ", std::to_string(params_.num_of_regions_dark)); + stat.add("number blockage regions ", std::to_string(params_.num_of_regions_blockage)); + stat.add("number low visibility regions ", std::to_string(params_.num_of_regions_low_visibility)); + stat.add("number backlight regions ", std::to_string(params_.num_of_regions_backlight)); auto level = DiagnosticStatusWrapper::OK; if (params_.diagnostic_status < 0) { diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index bd067e00e4c4e..07c6cfd9a570d 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -5,6 +5,8 @@ 1.0.0 The ROS 2 imu_corrector package Yamato Ando + Taiki Yamada + Koji Minoda Apache License 2.0 Yamato Ando diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index 7ff4371f48a9a..a3b5268815f24 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -49,7 +49,7 @@ For the example of actual usage of this node, please refer to the [preprocessor. ### How to tuning timeout_sec and input_offset -The values in `timeout_sec` and `input_offset` are used in the timercallback to control concatenation timings. +The values in `timeout_sec` and `input_offset` are used in the timer_callback to control concatenation timings. - Assumptions - when the timer runs out, we concatenate the pointclouds in the buffer diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 42ca694235a00..eedc8e6bcb573 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,13 +22,12 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ----------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index ab7d4e0012304..f655a9245ca6d 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -46,7 +46,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; - size_t max_points_num_per_ring_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -54,21 +53,6 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); - bool isCluster( - const PointCloud2ConstPtr & input, std::pair data_idx_both_ends, int walk_size) - { - if (walk_size > num_points_threshold_) return true; - - auto first_point = reinterpret_cast(&input->data[data_idx_both_ends.first]); - auto last_point = reinterpret_cast(&input->data[data_idx_both_ends.second]); - - const auto x = first_point->x - last_point->x; - const auto y = first_point->y - last_point->y; - const auto z = first_point->z - last_point->z; - - return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; - } - public: PCL_MAKE_ALIGNED_OPERATOR_NEW explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 09e24bedeb6b9..f30c02a97a6c0 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -88,6 +88,7 @@ namespace pointcloud_preprocessor { using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; +// cspell:ignore Yoshi /** \brief @b PointCloudDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index cdff4bf0162a4..6b1fc0f5bd976 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -15,41 +15,25 @@ import launch from launch.actions import DeclareLaunchArgument from launch.actions import LogInfo +from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch.substitutions import PythonExpression from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): ns = "pointcloud_preprocessor" pkg = "pointcloud_preprocessor" - # declare launch arguments - input_points_raw_list_param = DeclareLaunchArgument( - "input_points_raw_list", - default_value="['/points_raw']", - description="Input pointcloud topic_name list as a string_array. " - "To subscribe multiple topics, write as: \"['/points_raw0', '/points_raw1', ...]\"", - ) - - output_points_raw_param = DeclareLaunchArgument( - "output_points_raw", default_value="/points_raw/cropbox/filtered" - ) - - tf_output_frame_param = DeclareLaunchArgument("tf_output_frame", default_value="base_link") - - # set concat filter as a component - separate_concatenate_node_and_timesync_node_str = DeclareLaunchArgument( - "separate_concatenate_node_and_timesync_node", - default_value="false", - description="Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", - ) - separate_concatenate_node_and_timesync_node = ( - separate_concatenate_node_and_timesync_node_str.lower() == "true" + separate_concatenate_node_and_time_sync_node = LaunchConfiguration( + "separate_concatenate_node_and_time_sync_node" + ).perform(context) + is_separate_concatenate_node_and_time_sync_node = ( + separate_concatenate_node_and_time_sync_node.lower() == "true" ) - if not separate_concatenate_node_and_timesync_node: + if not is_separate_concatenate_node_and_time_sync_node: sync_and_concat_component = ComposableNode( package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", @@ -150,13 +134,30 @@ def generate_launch_description(): ] ) ) + return [container, log_info] - return launch.LaunchDescription( - [ - input_points_raw_list_param, - output_points_raw_param, - tf_output_frame_param, - container, - log_info, - ] + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg( + "input_points_raw_list", + ["/points_raw"], + "Input pointcloud topic_name list as a string_array. " + "To subscribe multiple topics, write as: \"['/points_raw0', '/points_raw1', ...]\"", ) + add_launch_arg("output_points_raw", "/points_raw/cropbox/filtered") + add_launch_arg("tf_output_frame", "base_link") + add_launch_arg( + "separate_concatenate_node_and_time_sync_node", + "true", + "Set True to separate concatenate node and time_sync node. which will cause to larger memory usage.", + ) + + return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index a65d14c0a1194..e3090f34d6854 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -31,6 +31,7 @@ pcl_msgs pcl_ros point_cloud_msg_wrapper + range-v3 rclcpp rclcpp_components sensor_msgs diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index d2570b9c4d786..e277b221a090d 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,10 +14,18 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "pointcloud_preprocessor/utility/utilities.hpp" + +#include +#include +#include + #include #include +#include #include + namespace pointcloud_preprocessor { RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) @@ -40,8 +48,6 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("object_length_threshold", 0.1)); num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); - max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); } using std::placeholders::_1; @@ -61,122 +67,252 @@ void RingOutlierFilterComponent::faster_filter( } stop_watch_ptr_->toc("processing_time", true); - output.point_step = sizeof(PointXYZI); - output.data.resize(output.point_step * input->width); - size_t output_size = 0; + // The ring_outlier_filter specifies the expected input point cloud format, + // however, we want to verify the input is correct and make failures explicit. + auto getFieldOffsetSafely = [=]( + const std::string & field_name, + const pcl::PCLPointField::PointFieldTypes expected_type) -> size_t { + const auto field_index = pcl::getFieldIndex(*input, field_name); + if (field_index == -1) { + RCLCPP_ERROR(get_logger(), "Field %s not found in input point cloud", field_name.c_str()); + return -1UL; + } - const auto ring_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; - const auto azimuth_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Azimuth)).offset; - const auto distance_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; - const auto intensity_offset = - input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; - - std::vector> ring2indices; - ring2indices.reserve(max_rings_num_); - - for (uint16_t i = 0; i < max_rings_num_; i++) { - ring2indices.push_back(std::vector()); - ring2indices.back().reserve(max_points_num_per_ring_); - } + auto field = (*input).fields.at(field_index); + if (field.datatype != expected_type) { + RCLCPP_ERROR( + get_logger(), "Field %s has unexpected type %d (expected %d)", field_name.c_str(), + field.datatype, expected_type); + return -1UL; + } - for (size_t data_idx = 0; data_idx < input->data.size(); data_idx += input->point_step) { - const uint16_t ring = *reinterpret_cast(&input->data[data_idx + ring_offset]); - ring2indices[ring].push_back(data_idx); + return static_cast(field.offset); + }; + + // as per the specification of this node, these fields must be present in the input + const auto ring_offset = getFieldOffsetSafely("ring", pcl::PCLPointField::UINT16); + const auto azimuth_offset = getFieldOffsetSafely("azimuth", pcl::PCLPointField::FLOAT32); + const auto distance_offset = getFieldOffsetSafely("distance", pcl::PCLPointField::FLOAT32); + const auto intensity_offset = getFieldOffsetSafely("intensity", pcl::PCLPointField::FLOAT32); + + if ( + ring_offset == -1UL || azimuth_offset == -1UL || distance_offset == -1UL || + intensity_offset == -1UL) { + RCLCPP_ERROR(get_logger(), "One or more required fields are missing in input point cloud"); + return; } - // walk range: [walk_first_idx, walk_last_idx] - int walk_first_idx = 0; - int walk_last_idx = -1; + // The initial implementation of ring outlier filter looked like this: + // 1. Iterate over the input cloud and group point indices by ring + // 2. For each ring: + // 2.1. iterate over the ring points, and group points belonging to the same "walk" + // 2.2. when a walk is closed, copy indexed points to the output cloud if the walk is long + // enough. + // + // Because LIDAR data is naturally "azimuth-major order" and not "ring-major order", such + // implementation is not cache friendly at all, and has negative impact on all the other nodes. + // + // To tackle this issue, the algorithm has been rewritten so that points would be accessed in + // order. To do so, all rings are being processing simultaneously instead of separately. The + // overall logic is still the same. + + // ad-hoc struct to store finished walks information (for isCluster()) + struct WalkInfo + { + size_t id; + int num_points; + float first_point_distance; + float first_point_azimuth; + float last_point_distance; + float last_point_azimuth; + }; + + // ad-hoc struct to keep track of each ring current walk + struct RingWalkInfo + { + WalkInfo first_walk; + WalkInfo current_walk; + }; + + // helper functions + + // check if walk is a valid cluster + const float object_length_threshold2 = object_length_threshold_ * object_length_threshold_; + auto isCluster = [=](const WalkInfo & walk_info) { + // A cluster is a walk which has many points or is long enough + if (walk_info.num_points > num_points_threshold_) return true; + + // Using the law of cosines, the distance between 2 points can be written as: + // |p2-p1|^2 = d1^2 + d2^2 - 2*d1*d2*cos(a) + // where d1 and d2 are the distance attribute of p1 and p2, and 'a' the azimuth diff between + // the 2 points. + const float dist2 = + walk_info.first_point_distance * walk_info.first_point_distance + + walk_info.last_point_distance * walk_info.last_point_distance - + 2 * walk_info.first_point_distance * walk_info.last_point_distance * + std::cos((walk_info.last_point_azimuth - walk_info.first_point_azimuth) * (M_PI / 18000.0)); + return dist2 > object_length_threshold2; + }; + + // check if 2 points belong to the same walk + auto isSameWalk = + [=](float curr_distance, float curr_azimuth, float prev_distance, float prev_azimuth) { + float azimuth_diff = curr_azimuth - prev_azimuth; + azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + return std::max(curr_distance, prev_distance) < + std::min(curr_distance, prev_distance) * distance_ratio_ && + azimuth_diff < 100.f; + }; + + // tmp vectors to keep track of walk/ring state while processing points in order (cache efficient) + std::vector rings; // info for each LiDAR ring + std::vector points_walk_id; // for each input point, the walk index associated with it + std::vector + walks_cluster_status; // for each generated walk, stores whether it is a cluster + + size_t latest_walk_id = -1UL; // ID given to the latest walk created + + // initialize each ring with two empty walks (first and current walk) + rings.resize(max_rings_num_, RingWalkInfo{{-1UL, 0, 0, 0, 0, 0}, {-1UL, 0, 0, 0, 0, 0}}); + // points are initially associated to no walk (-1UL) + points_walk_id.resize(input->width * input->height, -1UL); + walks_cluster_status.reserve( + max_rings_num_ * 2); // In the worst case, this could grow to the number of input points + + int invalid_ring_count = 0; + + // Build walks and classify points + for (const auto & [raw_p, point_walk_id] : + ranges::views::zip(input->data | ranges::views::chunk(input->point_step), points_walk_id)) { + uint16_t ring_idx{}; + float curr_azimuth{}; + float curr_distance{}; + std::memcpy(&ring_idx, &raw_p.data()[ring_offset], sizeof(ring_idx)); + std::memcpy(&curr_azimuth, &raw_p.data()[azimuth_offset], sizeof(curr_azimuth)); + std::memcpy(&curr_distance, &raw_p.data()[distance_offset], sizeof(curr_distance)); + + if (ring_idx >= max_rings_num_) { + // Either the data is corrupted or max_rings_num_ is not set correctly + // Note: point_walk_id == -1 so the point will be filtered out + ++invalid_ring_count; + continue; + } - for (const auto & indices : ring2indices) { - if (indices.size() < 2) continue; + auto & ring = rings[ring_idx]; + if (ring.current_walk.id == -1UL) { + // first walk ever for this ring. It is both the first and current walk of the ring. + ring.first_walk = + WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; + ring.current_walk = ring.first_walk; + point_walk_id = latest_walk_id; + continue; + } - walk_first_idx = 0; - walk_last_idx = -1; + auto & walk = ring.current_walk; + if (isSameWalk( + curr_distance, curr_azimuth, walk.last_point_distance, walk.last_point_azimuth)) { + // current point is part of previous walk + walk.num_points += 1; + walk.last_point_distance = curr_distance; + walk.last_point_azimuth = curr_azimuth; + point_walk_id = walk.id; + } else { + // previous walk is finished, start a new one + + // check and store whether the previous walks is a cluster + if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); + walks_cluster_status.at(walk.id) = isCluster(walk); + + ring.current_walk = + WalkInfo{++latest_walk_id, 1, curr_distance, curr_azimuth, curr_distance, curr_azimuth}; + point_walk_id = latest_walk_id; + } + } - for (size_t idx = 0U; idx < indices.size() - 1; ++idx) { - const size_t & current_data_idx = indices[idx]; - const size_t & next_data_idx = indices[idx + 1]; - walk_last_idx = idx; + // So far, we have processed ring points as if rings were not circular. Of course, the last and + // first points of a ring could totally be part of the same walk. When such thing happens, we need + // to merge the two walks + for (auto & ring : rings) { + if (ring.current_walk.id == -1UL) { + continue; + } - // if(std::abs(iter->distance - (iter+1)->distance) <= std::sqrt(iter->distance) * 0.08) + const auto & walk = ring.current_walk; + if (walk.id >= walks_cluster_status.size()) walks_cluster_status.resize(walk.id + 1, false); + walks_cluster_status.at(walk.id) = isCluster(walk); - const float & current_azimuth = - *reinterpret_cast(&input->data[current_data_idx + azimuth_offset]); - const float & next_azimuth = - *reinterpret_cast(&input->data[next_data_idx + azimuth_offset]); - float azimuth_diff = next_azimuth - current_azimuth; - azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff; + if (ring.first_walk.id == ring.current_walk.id) { + continue; + } - const float & current_distance = - *reinterpret_cast(&input->data[current_data_idx + distance_offset]); - const float & next_distance = - *reinterpret_cast(&input->data[next_data_idx + distance_offset]); + auto & first_walk = ring.first_walk; + auto & last_walk = ring.current_walk; + + // check if the two walks are connected + if (isSameWalk( + first_walk.first_point_distance, first_walk.first_point_azimuth, + last_walk.last_point_distance, last_walk.last_point_azimuth)) { + // merge + auto combined_num_points = first_walk.num_points + last_walk.num_points; + first_walk.first_point_distance = last_walk.first_point_distance; + first_walk.first_point_azimuth = last_walk.first_point_azimuth; + first_walk.num_points = combined_num_points; + last_walk.last_point_distance = first_walk.last_point_distance; + last_walk.last_point_azimuth = first_walk.last_point_azimuth; + last_walk.num_points = combined_num_points; + + walks_cluster_status.at(first_walk.id) = isCluster(first_walk); + walks_cluster_status.at(last_walk.id) = isCluster(last_walk); + } + } - if ( - std::max(current_distance, next_distance) < - std::min(current_distance, next_distance) * distance_ratio_ && - azimuth_diff < 100.f) { - continue; // Determined to be included in the same walk + // finally copy points + output.point_step = sizeof(PointXYZI); + output.data.resize(output.point_step * input->width * input->height); + size_t output_size = 0; + if (transform_info.need_transform) { + for (const auto & [raw_p, point_walk_id] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_id)) { + // Filter out points on invalid rings and points not in a cluster + if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { + continue; } - if (isCluster( - input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), - walk_last_idx - walk_first_idx + 1)) { - for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); - - if (transform_info.need_transform) { - Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); - p = transform_info.eigen_transform * p; - output_ptr->x = p[0]; - output_ptr->y = p[1]; - output_ptr->z = p[2]; - } else { - *output_ptr = *input_ptr; - } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; - - output_size += output.point_step; - } + // assume binary representation of input point is compatible with PointXYZ* + PointXYZI out_point; + std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); + + Eigen::Vector4f p(out_point.x, out_point.y, out_point.z, 1); + p = transform_info.eigen_transform * p; + out_point.x = p[0]; + out_point.y = p[1]; + out_point.z = p[2]; + // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI + std::memcpy( + &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); + + std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + output_size += sizeof(PointXYZI); + } + } else { + for (const auto & [raw_p, point_walk_id] : ranges::views::zip( + input->data | ranges::views::chunk(input->point_step), points_walk_id)) { + // Filter out points on invalid rings and points not in a cluster + if (point_walk_id == -1UL || !walks_cluster_status.at(point_walk_id)) { + continue; } - walk_first_idx = idx + 1; - } + PointXYZI out_point; + std::memcpy(&out_point, raw_p.data(), sizeof(PointXYZI)); + // FIXME(VRichardJP) tmp fix because input data does not have the same layout than PointXYZI + std::memcpy( + &out_point.intensity, &raw_p.data()[intensity_offset], sizeof(out_point.intensity)); - if (walk_first_idx > walk_last_idx) continue; - - if (isCluster( - input, std::make_pair(indices[walk_first_idx], indices[walk_last_idx]), - walk_last_idx - walk_first_idx + 1)) { - for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto output_ptr = reinterpret_cast(&output.data[output_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); - - if (transform_info.need_transform) { - Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); - p = transform_info.eigen_transform * p; - output_ptr->x = p[0]; - output_ptr->y = p[1]; - output_ptr->z = p[2]; - } else { - *output_ptr = *input_ptr; - } - const float & intensity = - *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - output_ptr->intensity = intensity; - - output_size += output.point_step; - } + std::memcpy(&output.data[output_size], &out_point, sizeof(PointXYZI)); + + output_size += sizeof(PointXYZI); } } - output.data.resize(output_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -196,6 +332,12 @@ void RingOutlierFilterComponent::faster_filter( sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + if (invalid_ring_count > 0) { + RCLCPP_WARN( + get_logger(), "%d points had ring index over max_rings_num (%d) and have been ignored.", + invalid_ring_count, max_rings_num_); + } + // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 7bb80164bb9a0..7b9ad47d3782c 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/localization.cpp src/motion.cpp src/operation_mode.cpp + src/perception.cpp src/planning.cpp src/routing.cpp src/vehicle.cpp @@ -25,6 +26,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" "default_ad_api::OperationModeNode" + "default_ad_api::PerceptionNode" "default_ad_api::PlanningNode" "default_ad_api::RoutingNode" "default_ad_api::VehicleNode" diff --git a/system/default_ad_api/document/images/autoware-state-table.drawio.svg b/system/default_ad_api/document/images/autoware-state-table.drawio.svg index ab21c1b865407..2b57830f6b869 100644 --- a/system/default_ad_api/document/images/autoware-state-table.drawio.svg +++ b/system/default_ad_api/document/images/autoware-state-table.drawio.svg @@ -3,14 +3,14 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="721px" + width="921px" height="201px" - viewBox="-0.5 -0.5 721 201" - content="<mxfile><diagram id="DGwK_OstZrDMA8GPxkck" name="Page-1">3Zpdk5owFIZ/DffyIbqXravbXnSm073Y61QiZBoTJwY/9tc3QEDMWafMmkYmOuPACVF4OEne92AQL7anF4F2xQ+eYRpEk+wUxM9BFM1mU/VZBc5NIJmlTSAXJGtC4SXwSt6xDk50tCQZ3l8dKDmnkuyug2vOGF7LqxgSgh+vD9twev2rO5RjEHhdIwqjbySThY6G6dOl4RsmeaF/eh7NmoYtag/WV7IvUMaPvVC8DOKF4Fw2W9vTAtOKXcul6be60dqdmMBMDukQ6TM+IFrqi9MnJs/t1QpesgxXHSZB/PVYEIlfd2hdtR7V7VWxQm6p2gvV5oZQuuCUi7pvvJlW7yrOmezFm5eK76Xgf3CvJa1fqkWfGBYSn25eXdgxU7mG+RZLcVaH6A5pojHrPEv17rF309o7UfTu15OOIZ0neffNF5RqQ9O8QRaAfUNEEpavuPjFS4k94mxg7pj+i3NkgXMMOFOuRip5R5JwVhNGXsGO5te0E4ewEwBbcayS2kPOyeRxnKeAM99h0Wb0Vq2nHoGeRo8DnQLQqJS8YzxBB0Qo+k194m2uii55zwBvwtSqWM3X9SxyD2QLaML0c4LBBpo5nFtLxsZIZT6QSphYwAIFasm6nFEQHg3HXI6HwrGRMq0fGutwMtmEE5dwwttwRpg3g0VzB/EuONCelGyPZRAtGo9MDiNgZEowp8YihM6iBjQ2JkOX79QGEyhMx5orkdO5BuqaDWFjmYbN9TtyOoygrtlLvns4FNN1DB5HNkRNCFUN47IGMkY0LjVN+x39oYTo/k4DZoGKaZsGJ4wVKlDMSFGOEIpLxxRBEfPdmvwdlWX/ZB3bxpL/UYEVlWxdBP6V/MyF0ukIh7XVnxRZKACMivBD51AoXi+PZJYsrx76+Uva6cQMy6rPyiX4ncpORRL0G18aH/bCEfUXslNT90FFdmXJ1I2asdOnt1Dte0QWqOOhc4QN4RZDy+ARWrPc41ISx9B3eETWdP9Dyc5tkIVmw2Oyg1czGyWnGPoLj9CC6q/TRQwaC4/Qmg+q3KKFTsJjtP/Ro6ndy79K67beX3Pj5V8=</diagram></mxfile>" + viewBox="-0.5 -0.5 921 201" + content="<mxfile><diagram id="DGwK_OstZrDMA8GPxkck" name="Page-1">3ZpNc5swEIZ/Sw/cAdnEObbOR3voTKc55JhRjWw0FZJHFraTX18JhI1ZMmESVWFkz9iw4ksPi3bfRRFalsd7ibfFT5ETFqVxfozQTZSmyXw+13/G8txYsuukMWwkze1GZ8MDfSHWGFtrRXOyu9hQCcEU3V4aV4JzslIXNiylOFxuthbs8qxbvCHA8LDCDFofaa4Ka02y63PDd0I3hT31Ir1qGkrcbmx7sitwLg4dE7qN0FIKoZql8rgkzMBruTT73b3SerowSbgas0Nqr3iPWWU7Zy9MPbe9laLiOTE7xBH6diioIg9bvDKtB31/ta1QJdNriV5cU8aWgglZ74vWc/M1dsFVx958tH2npPhLOi1Z/dEtsCu2d3siFTl2TLZr90SURMlnvYltXcws5tbP7Oqhc9PaO1F07te1tWHrJ5vTkc8o9YKl+QpZAPYRU0X55k7I36JSJCDOPcwnpm9xTh1wRoAzE/pJpS9YUcFrwjgo2OnikvbMI+wZgK05GqcOkPMs/jzOc8BZbIlsPbrUATUg0PP0TdDof4HOAGhcKXFiHOM9pgz/YSHx7kdFn459BXhTrqOiGa/rUeQjkB2gSbL3JQwu0Czg2FpxPkUqi5FUkpkDLDBBrfjJZzSEz4bTD8dj4bhwmVYPTfVx6rNJYp9wktfhTNBvRifNJ4gfggPlScV3REXpstHIdD8BRv0UzKuwSKCyqAFNjcnY8J25YAIT06n6Sup1rIF5zZryqQzD/fiden2MYF5D2O6D+bQDKH3VMfQcITQAxUVSk8LAvcZToNLXBqNHFxeuksKIrWQ1QSg+ZUEKI/UPZznepHTpO4u1LuLaUBURV3xVROHVtfrRwOsTDguIvxh2oHInRfhTx1CYoZ3fO9zyjXmzFS5prwMzrB3e6FQ4bFf2Wd0YKBZ+bcTGvcAsXMhelctA2fHOkXKZNGOvryhhth8QWZAdjx0jXCRuCEqGgND2axo+U2IEdUdAZPvqf4Ds4DvHhQuyUGwETHYomg2idVFXQVBfBIQWlDi9BjEoLAJC238b4xctVBIBo/Wp0RCUEHZix5d6TRMW28icKcOlgdj8gtXYzAo5aApPK41QCvZEuJkVkpt99YHqY7mqVQK0AzdgfFl7IBtzNZtGr55nqtZtnfm+6PYf</diagram></mxfile>" > - + @@ -62,13 +62,13 @@ routing state - +
@@ -77,16 +77,16 @@
- operation mode + operation mode - +
@@ -95,7 +95,7 @@
- auto mode available + auto mode available @@ -260,49 +260,31 @@ finalizing - +
- stop + else
- stop + else - +
-
-
- not stop -
-
-
- - not stop - - - - - - -
@@ -311,16 +293,16 @@
- false + false - +
@@ -329,7 +311,7 @@
- true + true @@ -440,18 +422,38 @@ Finalizing - + - - + + + + + + +
+
+
+ mode != stop && autoware_control_enabled == true +
+
+
+ + + mode != stop && autoware_control_enabled == true + + + - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index fdb79d517206d..57a8557eb7e0a 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -47,6 +47,7 @@ def generate_launch_description(): create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), create_api_node("operation_mode", "OperationModeNode"), + create_api_node("perception", "PerceptionNode"), create_api_node("planning", "PlanningNode"), create_api_node("routing", "RoutingNode"), create_api_node("vehicle", "VehicleNode"), diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 032335fdddfd2..dfceeabe41122 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -28,6 +28,7 @@ nav_msgs rclcpp rclcpp_components + shape_msgs std_srvs tier4_api_msgs tier4_control_msgs diff --git a/system/default_ad_api/src/perception.cpp b/system/default_ad_api/src/perception.cpp new file mode 100644 index 0000000000000..61d09444c70a4 --- /dev/null +++ b/system/default_ad_api/src/perception.cpp @@ -0,0 +1,93 @@ +// 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. + +#include "perception.hpp" + +#include + +namespace default_ad_api +{ + +using DynamicObjectArray = autoware_ad_api::perception::DynamicObjectArray; +using ObjectClassification = autoware_adapi_v1_msgs::msg::ObjectClassification; +using DynamicObject = autoware_adapi_v1_msgs::msg::DynamicObject; +using DynamicObjectPath = autoware_adapi_v1_msgs::msg::DynamicObjectPath; +using API_Shape = shape_msgs::msg::SolidPrimitive; +using Shape = autoware_auto_perception_msgs::msg::Shape; + +std::unordered_map shape_type_ = { + {Shape::BOUNDING_BOX, API_Shape::BOX}, + {Shape::CYLINDER, API_Shape::CYLINDER}, + {Shape::POLYGON, API_Shape::PRISM}, +}; + +PerceptionNode::PerceptionNode(const rclcpp::NodeOptions & options) : Node("perception", options) +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_object_recognized_); + adaptor.init_sub(sub_object_recognized_, this, &PerceptionNode::object_recognize); +} + +uint8_t PerceptionNode::mapping( + std::unordered_map hash_map, uint8_t input, uint8_t default_value) +{ + if (hash_map.find(input) == hash_map.end()) { + return default_value; + } else { + return hash_map[input]; + } +} + +void PerceptionNode::object_recognize( + const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg) +{ + DynamicObjectArray::Message objects; + objects.header = msg->header; + for (const auto & msg_object : msg->objects) { + DynamicObject object; + object.id = msg_object.object_id; + object.existence_probability = msg_object.existence_probability; + for (const auto & msg_classification : msg_object.classification) { + ObjectClassification classification; + classification.label = msg_classification.label; + classification.probability = msg_classification.probability; + object.classification.insert(object.classification.begin(), classification); + } + object.kinematics.pose = msg_object.kinematics.initial_pose_with_covariance.pose; + object.kinematics.twist = msg_object.kinematics.initial_twist_with_covariance.twist; + object.kinematics.accel = msg_object.kinematics.initial_acceleration_with_covariance.accel; + for (const auto & msg_predicted_path : msg_object.kinematics.predicted_paths) { + DynamicObjectPath predicted_path; + for (const auto & msg_path : msg_predicted_path.path) { + predicted_path.path.insert(predicted_path.path.begin(), msg_path); + } + predicted_path.time_step = msg_predicted_path.time_step; + predicted_path.confidence = msg_predicted_path.confidence; + object.kinematics.predicted_paths.insert( + object.kinematics.predicted_paths.begin(), predicted_path); + } + object.shape.type = mapping(shape_type_, msg_object.shape.type, API_Shape::PRISM); + object.shape.dimensions = { + msg_object.shape.dimensions.x, msg_object.shape.dimensions.y, msg_object.shape.dimensions.z}; + object.shape.polygon = msg_object.shape.footprint; + objects.objects.insert(objects.objects.begin(), object); + } + + pub_object_recognized_->publish(objects); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PerceptionNode) diff --git a/system/default_ad_api/src/perception.hpp b/system/default_ad_api/src/perception.hpp new file mode 100644 index 0000000000000..d1c71fdb08025 --- /dev/null +++ b/system/default_ad_api/src/perception.hpp @@ -0,0 +1,52 @@ +// 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 PERCEPTION_HPP_ +#define PERCEPTION_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class PerceptionNode : public rclcpp::Node +{ +public: + explicit PerceptionNode(const rclcpp::NodeOptions & options); + +private: + Pub pub_object_recognized_; + Sub sub_object_recognized_; + void object_recognize(const perception_interface::ObjectRecognition::Message::ConstSharedPtr msg); + uint8_t mapping( + std::unordered_map hash_map, uint8_t input, uint8_t default_value); +}; + +} // namespace default_ad_api + +#endif // PERCEPTION_HPP_ diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index d22e065d37c5f..61d5b2dde6103 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -154,7 +154,7 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; - } else if (map_projector_info_->type == "UTM") { + } else if (map_projector_info_->type == "LocalCartesianUTM") { lanelet::GPSPoint position{ map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; lanelet::Origin origin{position}; diff --git a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp index 183f86baa2a08..15702852a0e50 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp @@ -24,6 +24,25 @@ #include +/** + * @brief Struct for storing process information + */ +struct ProcessInfo +{ + std::string processId; + std::string userName; + std::string priority; + std::string niceValue; + std::string virtualImage; + std::string residentSize; + std::string sharedMemSize; + std::string processStatus; + std::string cpuUsage; + std::string memoryUsage; + std::string cpuTime; + std::string commandName; +}; + class DiagTask : public diagnostic_updater::DiagnosticTask { public: @@ -46,18 +65,18 @@ class DiagTask : public diagnostic_updater::DiagnosticTask if (level_ != DiagStatus::OK) { stat.add("content", content_); } else { - stat.add("COMMAND", command_); - stat.add("%CPU", cpu_); - stat.add("%MEM", mem_); - stat.add("PID", pid_); - stat.add("USER", user_); - stat.add("PR", pr_); - stat.add("NI", ni_); - stat.add("VIRT", virt_); - stat.add("RES", res_); - stat.add("SHR", shr_); - stat.add("S", s_); - stat.add("TIME+", time_); + stat.add("COMMAND", info_.commandName); + stat.add("%CPU", info_.cpuUsage); + stat.add("%MEM", info_.memoryUsage); + stat.add("PID", info_.processId); + stat.add("USER", info_.userName); + stat.add("PR", info_.priority); + stat.add("NI", info_.niceValue); + stat.add("VIRT", info_.virtualImage); + stat.add("RES", info_.residentSize); + stat.add("SHR", info_.sharedMemSize); + stat.add("S", info_.processStatus); + stat.add("TIME+", info_.cpuTime); } stat.summary(level_, message_); } @@ -85,95 +104,17 @@ class DiagTask : public diagnostic_updater::DiagnosticTask } /** - * @brief set process id - * @param [in] pid process id - */ - void setProcessId(const std::string & pid) { pid_ = pid; } - - /** - * @brief set user name - * @param [in] user user name - */ - void setUserName(const std::string & user) { user_ = user; } - - /** - * @brief set priority - * @param [in] pr priority - */ - void setPriority(const std::string & pr) { pr_ = pr; } - - /** - * @brief set nice value - * @param [in] ni nice value - */ - void setNiceValue(const std::string & ni) { ni_ = ni; } - - /** - * @brief set virtual image - * @param [in] virt virtual image - */ - void setVirtualImage(const std::string & virt) { virt_ = virt; } - - /** - * @brief set resident size - * @param [in] res resident size + * @brief Set process information + * @param [in] info Process information */ - void setResidentSize(const std::string & res) { res_ = res; } - - /** - * @brief set shared mem size - * @param [in] shr shared mem size - */ - void setSharedMemSize(const std::string & shr) { shr_ = shr; } - - /** - * @brief set process status - * @param [in] s process status - */ - void setProcessStatus(const std::string & s) { s_ = s; } - - /** - * @brief set CPU usage - * @param [in] cpu CPU usage - */ - void setCPUUsage(const std::string & cpu) { cpu_ = cpu; } - - /** - * @brief set memory usage - * @param [in] mem memory usage - */ - void setMemoryUsage(const std::string & mem) { mem_ = mem; } - - /** - * @brief set CPU time - * @param [in] time CPU time - */ - void setCPUTime(const std::string & time) { time_ = time; } - - /** - * @brief set Command name/line - * @param [in] command Command name/line - */ - void setCommandName(const std::string & command) { command_ = command; } + void setProcessInformation(const struct ProcessInfo & info) { info_ = info; } private: int level_; //!< @brief Diagnostics error level std::string message_; //!< @brief Diagnostics status message std::string error_command_; //!< @brief Error command std::string content_; //!< @brief Error content - - std::string pid_; //!< @brief Process Id - std::string user_; //!< @brief User Name - std::string pr_; //!< @brief Priority - std::string ni_; //!< @brief Nice value - std::string virt_; //!< @brief Virtual Image (kb) - std::string res_; //!< @brief Resident size (kb) - std::string shr_; //!< @brief Shared Mem size (kb) - std::string s_; //!< @brief Process Status - std::string cpu_; //!< @brief CPU usage - std::string mem_; //!< @brief Memory usage - std::string time_; //!< @brief CPU Time - std::string command_; //!< @brief Command name/line + struct ProcessInfo info_; //!< @brief Struct for storing process information }; #endif // SYSTEM_MONITOR__PROCESS_MONITOR__DIAG_TASK_HPP_ diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f78fc00c430dc..f8ee314e5a0ef 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -401,7 +401,7 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) bp::pipe err_pipe{err_fd[0], err_fd[1]}; bp::ipstream is_err{std::move(err_pipe)}; - bp::child c("sort -r -k 10", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); + bp::child c("sort -r -k 10 -n", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); c.wait(); if (c.exit_code() != 0) { is_err >> os.rdbuf(); @@ -451,27 +451,21 @@ void ProcessMonitor::getTopratedProcesses( return; } - std::vector list; std::string line; int index = 0; while (std::getline(is_out, line) && !line.empty()) { - boost::trim_left(line); - boost::split(list, line, boost::is_space(), boost::token_compress_on); + std::istringstream stream(line); + + ProcessInfo info; + stream >> info.processId >> info.userName >> info.priority >> info.niceValue >> + info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> + info.cpuUsage >> info.memoryUsage >> info.cpuTime; + + std::getline(stream, info.commandName); tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); - tasks->at(index)->setProcessId(list[0]); - tasks->at(index)->setUserName(list[1]); - tasks->at(index)->setPriority(list[2]); - tasks->at(index)->setNiceValue(list[3]); - tasks->at(index)->setVirtualImage(list[4]); - tasks->at(index)->setResidentSize(list[5]); - tasks->at(index)->setSharedMemSize(list[6]); - tasks->at(index)->setProcessStatus(list[7]); - tasks->at(index)->setCPUUsage(list[8]); - tasks->at(index)->setMemoryUsage(list[9]); - tasks->at(index)->setCPUTime(list[10]); - tasks->at(index)->setCommandName(list[11]); + tasks->at(index)->setProcessInformation(info); ++index; } } @@ -521,7 +515,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { diff --git a/system/topic_state_monitor/Readme.md b/system/topic_state_monitor/README.md similarity index 100% rename from system/topic_state_monitor/Readme.md rename to system/topic_state_monitor/README.md