diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 0000000000000..d1688ee2df9a9 --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,159 @@ +### Copied from .github/CODEOWNERS-manual ### + +### Automatically generated from package.xml ### +control/external_cmd_selector/* kenji.miyake@tier4.jp +control/obstacle_collision_checker/* kenji.miyake@tier4.jp +control/vehicle_cmd_gate/* takamasa.horibe@tier4.jp +control/trajectory_follower/* takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/trajectory_follower_nodes/* takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/joy_controller/* kenji.miyake@tier4.jp +control/lane_departure_checker/* kyoichi.sugahara@tier4.jp +control/shift_decider/* takamasa.horibe@tier4.jp +control/pure_pursuit/* takamasa.horibe@tier4.jp +control/operation_mode_transition_manager/* takamasa.horibe@tier4.jp +control/control_performance_analysis/* ali.boyali@tier4.jp berkay@leodrive.ai +common/autoware_ad_api_msgs/* isamu.takagi@tier4.jp +common/motion_common/* christopherj.ho@gmail.com +common/component_interface_specs/* isamu.takagi@tier4.jp +common/motion_testing/* christopherj.ho@gmail.com +common/autoware_auto_perception_rviz_plugin/* opensource@apex.ai taichi.higashide@tier4.jp +common/autoware_auto_tf2/* jit.ray.c@gmail.com +common/signal_processing/* takayuki.murooka@tier4.jp +common/tier4_perception_rviz_plugin/* yukihiro.saito@tier4.jp +common/path_distance_calculator/* isamu.takagi@tier4.jp +common/kalman_filter/* yukihiro.saito@tier4.jp +common/tier4_traffic_light_rviz_plugin/* satoshi.ota@tier4.jp +common/vehicle_constants_manager/* mfc@leodrive.ai +common/component_interface_utils/* isamu.takagi@tier4.jp +common/tier4_planning_rviz_plugin/* yukihiro.saito@tier4.jp +common/perception_utils/* takayuki.murooka@tier4.jp +common/web_controller/* yukihiro.saito@tier4.jp +common/goal_distance_calculator/* taiki.tanaka@tier4.jp +common/motion_utils/* yutaka.shimizu@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp +common/tier4_debug_tools/* kenji.miyake@tier4.jp +common/tier4_localization_rviz_plugin/* isamu.takagi@tier4.jp +common/autoware_point_types/* taichi.higashide@tier4.jp +common/tier4_screen_capture_rviz_plugin/* taiki.tanaka@tier4.jp +common/tier4_control_rviz_plugin/* taiki.tanaka@tier4.jp +common/osqp_interface/* maxime.clement@tier4.jp +common/tier4_simulated_clock_rviz_plugin/* maxime.clement@tier4.jp +common/neural_networks_provider/* ambroise.vincent@arm.com +common/global_parameter_loader/* kenji.miyake@tier4.jp +common/autoware_auto_common/* opensource@apex.ai +common/tvm_utility/* ambroise.vincent@arm.com +common/autoware_auto_geometry/* opensource@apex.ai +common/autoware_ad_api_specs/* isamu.takagi@tier4.jp +common/tier4_state_rviz_plugin/* hiroki.ota@tier4.jp +common/tier4_datetime_rviz_plugin/* isamu.takagi@tier4.jp +common/grid_map_utils/* maxime.clement@tier4.jp +common/tier4_autoware_utils/* takamasa.horibe@tier4.jp kenji.miyake@tier4.jp +common/fake_test_node/* opensource@apex.ai +common/autoware_testing/* adam.dabrowski@robotec.ai +common/tier4_api_utils/* isamu.takagi@tier4.jp +common/tier4_calibration_rviz_plugin/* tomoya.kimura@tier4.jp +common/interpolation/* fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp +common/time_utils/* christopherj.ho@gmail.com +common/polar_grid/* yukihiro.saito@tier4.jp +common/tier4_vehicle_rviz_plugin/* yukihiro.saito@tier4.jp +launch/tier4_vehicle_launch/* yukihiro.saito@tier4.jp +launch/tier4_system_launch/* kenji.miyake@tier4.jp +launch/tier4_map_launch/* ryohsuke.mitsudome@tier4.jp +launch/tier4_control_launch/* takamasa.horibe@tier4.jp +launch/tier4_sensing_launch/* yukihiro.saito@tier4.jp +launch/tier4_autoware_api_launch/* isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp +launch/tier4_perception_launch/* yukihiro.saito@tier4.jp +launch/tier4_planning_launch/* takamasa.horibe@tier4.jp +launch/tier4_localization_launch/* yamato.ando@tier4.jp +launch/tier4_simulator_launch/* keisuke.shima@tier4.jp +evaluator/localization_evaluator/* dominik.jargot@robotec.ai +evaluator/kinematic_evaluator/* dominik.jargot@robotec.ai +map/map_loader/* ryohsuke.mitsudome@tier4.jp kenji.miyake@tier4.jp +map/map_tf_generator/* azumi.suzuki@tier4.jp +map/util/lanelet2_map_preprocessor/* ryohsuke.mitsudome@tier4.jp +simulator/simple_planning_simulator/* takamasa.horibe@tier4.jp +simulator/dummy_perception_publisher/* yukihiro.saito@tier4.jp +simulator/fault_injection/* keisuke.shima@tier4.jp +tools/simulator_test/simulator_compatibility_test/* shpark@morai.ai +sensing/livox/livox_tag_filter/* kenji.miyake@tier4.jp +sensing/pointcloud_preprocessor/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +sensing/probabilistic_occupancy_grid_map/* yukihiro.saito@tier4.jp +sensing/imu_corrector/* yamato.ando@tier4.jp +sensing/geo_pos_conv/* yamato.ando@tier4.jp +sensing/image_transport_decompressor/* yukihiro.saito@tier4.jp +sensing/vehicle_velocity_converter/* ryu.yamamoto@tier4.jp +sensing/image_diagnostics/* dai.nguyen@tier4.jp +sensing/tier4_pcl_extensions/* ryu.yamamoto@tier4.jp +sensing/gnss_poser/* yamato.ando@tier4.jp +planning/freespace_planner/* takamasa.horibe@tier4.jp +planning/obstacle_cruise_planner/* takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/planning_error_monitor/* yutaka.shimizu@tier4.jp +planning/surround_obstacle_checker/* satoshi.ota@tier4.jp +planning/costmap_generator/* kenji.miyake@tier4.jp +planning/behavior_path_planner/* zulfaqar.azmi@tier4.jp kosuke.takeuchi@tier4.jp kosuke.takeuchi@tier4.jp fumiya.watanabe@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp takayuki.murooka@tier4.jp +planning/obstacle_stop_planner/* satoshi.ota@tier4.jp +planning/behavior_velocity_planner/* mamoru.sobue@tier4.jp satoshi.ota@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp mamoru.sobue@tier4.jp yutaka.shimizu@tier4.jp kosuke.takeuchi@tier4.jp tomohito.ando@tier4.jp +planning/rtc_auto_approver/* fumiya.watanabe@tier4.jp +planning/scenario_selector/* kenji.miyake@tier4.jp +planning/planning_evaluator/* maxime.clement@tier4.jp +planning/rtc_interface/* fumiya.watanabe@tier4.jp +planning/planning_debug_tools/* takamasa.horibe@tier4.jp +planning/external_velocity_limit_selector/* satoshi.ota@tier4.jp +planning/freespace_planning_algorithms/* takamasa.horibe@tier4.jp +planning/obstacle_avoidance_planner/* takayuki.murooka@tier4.jp +planning/motion_velocity_smoother/* fumiya.watanabe@tier4.jp +planning/route_handler/* fumiya.watanabe@tier4.jp +planning/mission_planner/* ryohsuke.mitsudome@tier4.jp +perception/shape_estimation/* yukihiro.saito@tier4.jp +perception/front_vehicle_velocity_estimator/* satoshi.tanaka@tier4.jp +perception/traffic_light_visualization/* yukihiro.saito@tier4.jp +perception/detection_by_tracker/* yukihiro.saito@tier4.jp +perception/object_range_splitter/* yukihiro.saito@tier4.jp +perception/heatmap_visualizer/* kotaro.uetake@tier4.jp +perception/detected_object_feature_remover/* tomoya.kimura@tier4.jp +perception/crosswalk_traffic_light_estimator/* satoshi.ota@tier4.jp +perception/euclidean_cluster/* yukihiro.saito@tier4.jp +perception/image_projection_based_fusion/* yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/traffic_light_ssd_fine_detector/* daisuke.nishimatsu@tier4.jp +perception/lidar_centerpoint/* yusuke.muramatsu@tier4.jp +perception/map_based_prediction/* yutaka.shimizu@tier4.jp +perception/multi_object_tracker/* yukihiro.saito@tier4.jp jilada.eccleston@tier4.jp +perception/object_merger/* yukihiro.saito@tier4.jp +perception/elevation_map_loader/* kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp +perception/traffic_light_map_based_detector/* yukihiro.saito@tier4.jp +perception/tensorrt_yolo/* daisuke.nishimatsu@tier4.jp +perception/lidar_apollo_instance_segmentation/* yukihiro.saito@tier4.jp +perception/radar_fusion_to_detected_object/* satoshi.tanaka@tier4.jp +perception/radar_tracks_msgs_converter/* satoshi.tanaka@tier4.jp +perception/traffic_light_classifier/* yukihiro.saito@tier4.jp +perception/detected_object_validation/* yukihiro.saito@tier4.jp +perception/ground_segmentation/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +perception/occupancy_grid_map_outlier_filter/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +perception/compare_map_segmentation/* abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp +vehicle/steer_offset_estimator/* taiki.tanaka@tier4.jp +vehicle/raw_vehicle_cmd_converter/* takamasa.horibe@tier4.jp +vehicle/vehicle_info_util/* yamato.ando@tier4.jp +vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/* tomoya.kimura@tier4.jp +vehicle/external_cmd_converter/* takamasa.horibe@tier4.jp +localization/pose_initializer/* yamato.ando@tier4.jp +localization/ndt_pcl_modified/* yamato.ando@tier4.jp +localization/pose2twist/* yamato.ando@tier4.jp +localization/ndt_scan_matcher/* yamato.ando@tier4.jp +localization/gyro_odometer/* yamato.ando@tier4.jp +localization/localization_error_monitor/* yamato.ando@tier4.jp +localization/initial_pose_button_panel/* yamato.ando@tier4.jp +localization/twist2accel/* koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/ndt/* yamato.ando@tier4.jp +localization/ekf_localizer/* takamasa.horibe@tier4.jp koji.minoda@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/* koji.minoda@tier4.jp yamato.ando@tier4.jp +system/default_ad_api_helpers/ad_api_adaptors/* isamu.takagi@tier4.jp +system/default_ad_api_helpers/automatic_pose_initializer/* isamu.takagi@tier4.jp +system/velodyne_monitor/* fumihito.ito@tier4.jp +system/dummy_infrastructure/* kenji.miyake@tier4.jp +system/topic_state_monitor/* kenji.miyake@tier4.jp +system/bluetooth_monitor/* fumihito.ito@tier4.jp +system/ad_service_state_monitor/* kenji.miyake@tier4.jp +system/system_monitor/* fumihito.ito@tier4.jp +system/emergency_handler/* kenji.miyake@tier4.jp +system/dummy_diag_publisher/* kenji.miyake@tier4.jp +system/system_error_monitor/* kenji.miyake@tier4.jp +system/default_ad_api/* isamu.takagi@tier4.jp diff --git a/.github/CODEOWNERS-manual b/.github/CODEOWNERS-manual new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 31f869b6c8912..be25169b1604e 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -12,6 +12,7 @@ - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md - source: .github/dependabot.yaml - source: .github/stale.yml + - source: .github/workflows/cancel-previous-workflows.yaml - source: .github/workflows/github-release.yaml - source: .github/workflows/pre-commit.yaml - source: .github/workflows/pre-commit-optional.yaml @@ -81,8 +82,8 @@ include:" {source} - source: .github/workflows/check-build-depends.yaml pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {source} + - source: .github/workflows/update-codeowners-from-packages.yaml - source: codecov.yaml - repository: autowarefoundation/autoware-documentation diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 3181aafbbc7c7..248dbb09b61a4 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -23,9 +23,6 @@ jobs: container: ghcr.io/autowarefoundation/autoware-universe:humble-latest build-depends-repos: build_depends.humble.repos steps: - - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.10.0 - - name: Check out repository uses: actions/checkout@v3 with: @@ -81,11 +78,20 @@ jobs: id: get-modified-packages uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + - name: Get modified files + id: get-modified-files + uses: tj-actions/changed-files@v28 + with: + files: | + **/*.cpp + **/*.hpp + - name: Run clang-tidy - if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 with: rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.humble.repos diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml new file mode 100644 index 0000000000000..aeb34c6a03dcf --- /dev/null +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -0,0 +1,14 @@ +name: cancel-previous-workflows + +on: + pull_request_target: + +jobs: + cancel-previous-workflows: + runs-on: ubuntu-latest + steps: + - name: Cancel previous runs + uses: styfle/cancel-workflow-action@0.10.0 + with: + workflow_id: all + all_but_latest: true diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 6c82d2bf0b950..600eaa3c5dcb4 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -3,16 +3,26 @@ name: check-build-depends on: pull_request: paths: - - build_depends.repos + - build_depends*.repos jobs: check-build-depends: runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest + container: ${{ matrix.container }} + strategy: + fail-fast: false + matrix: + rosdistro: + - galactic + - humble + include: + - rosdistro: galactic + container: ros:galactic + build-depends-repos: build_depends.repos + - rosdistro: humble + container: ros:humble + build-depends-repos: build_depends.humble.repos steps: - - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.10.0 - - name: Check out repository uses: actions/checkout@v3 @@ -26,6 +36,6 @@ jobs: - name: Build uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - rosdistro: galactic + rosdistro: ${{ matrix.rosdistro }} target-packages: ${{ steps.get-self-packages.outputs.self-packages }} - build-depends-repos: build_depends.repos + build-depends-repos: ${{ matrix.build-depends-repos }} diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml new file mode 100644 index 0000000000000..055d2b02a799a --- /dev/null +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -0,0 +1,33 @@ +name: update-codeowners-from-packages + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + check-secret: + uses: autowarefoundation/autoware-github-actions/.github/workflows/check-secret.yaml@v1 + secrets: + secret: ${{ secrets.APP_ID }} + + update-codeowners-from-packages: + needs: check-secret + if: ${{ needs.check-secret.outputs.set == 'true' }} + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run update-codeowners-from-packages + uses: autowarefoundation/autoware-github-actions/update-codeowners-from-packages@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + pr-labels: | + bot + update-codeowners-from-packages + auto-merge-method: squash diff --git a/.yamllint.yaml b/.yamllint.yaml index 6228c70f02da9..2c7bd088e2648 100644 --- a/.yamllint.yaml +++ b/.yamllint.yaml @@ -1,7 +1,6 @@ extends: default ignore: | - .clang-tidy *.param.yaml rules: diff --git a/NOTICE b/NOTICE index 981e257f9bd21..0ac1d22befa35 100644 --- a/NOTICE +++ b/NOTICE @@ -6,3 +6,9 @@ The Autoware Foundation (https://www.autoware.org/). This product includes code developed by TIER IV. Copyright 2017 TIER IV, Inc. + +This product includes code developed by AutoCore. +Copyright 2022 AutoCore Technology (Nanjing) Co., Ltd. + +This product includes code developed by Leo Drive. +Copyright 2022 Leo Drive Teknoloji A.Ş. diff --git a/common/bag_time_manager_rviz_plugin/CMakeLists.txt b/common/bag_time_manager_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..ccf9961fc55cc --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.14) +project(bag_time_manager_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/bag_time_manager_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${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 + icons + plugins +) diff --git a/common/bag_time_manager_rviz_plugin/README.md b/common/bag_time_manager_rviz_plugin/README.md new file mode 100644 index 0000000000000..2fad6b2204bb9 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/README.md @@ -0,0 +1,26 @@ +# bag_time_manager_rviz_plugin + +## Purpose + +This plugin allows publishing and controlling the ros bag time. + +## Output + +tbd. + +## HowToUse + +1. Start rviz and select panels/Add new panel. + + ![select_panel](./images/select_panels.png) + +2. Select BagTimeManagerPanel and press OK. + + ![select_manager_plugin](./images/add_bag_time_manager_panel.png) + +3. See bag_time_manager_rviz_plugin/BagTimeManagerPanel is added. + + ![manager_plugin](./images/bag_time_manager_panel.png) + +- Pause/Resume: pause/resume the clock. +- ApplyRate: apply rate of the clock. diff --git a/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png b/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png new file mode 100644 index 0000000000000..9431c2d422363 Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png differ diff --git a/common/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png b/common/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png new file mode 100644 index 0000000000000..8078c0ccbf34c Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png differ diff --git a/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png b/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png new file mode 100644 index 0000000000000..47ec05d5539c8 Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png differ diff --git a/common/bag_time_manager_rviz_plugin/images/select_panels.png b/common/bag_time_manager_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000..a691602c42c3c Binary files /dev/null and b/common/bag_time_manager_rviz_plugin/images/select_panels.png differ diff --git a/common/bag_time_manager_rviz_plugin/package.xml b/common/bag_time_manager_rviz_plugin/package.xml new file mode 100644 index 0000000000000..b12c24b728cde --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + bag_time_manager_rviz_plugin + 0.0.1 + Rviz plugin to publish and control the ros bag + Taiki Tanaka + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rosbag2_interfaces + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml b/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..2a3bbf667b715 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + Panel that publishes a service to modify its speed. + + + diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp new file mode 100644 index 0000000000000..dde3f10e9b554 --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -0,0 +1,117 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#include "bag_time_manager_panel.hpp" + +#include +#include +#include +#include + +namespace rviz_plugins +{ +BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // pause / resume + { + pause_button_ = new QPushButton("Pause"); + pause_button_->setToolTip("Pause/Resume ROS time."); + pause_button_->setStyleSheet("background-color: #00FF00;"); + pause_button_->setCheckable(true); + } + + // apply + { + apply_rate_button_ = new QPushButton("ApplyRate"); + apply_rate_button_->setToolTip("control ROS time rate."); + } + + // combo + { + rate_label_ = new QLabel("Rate:"); + rate_label_->setAlignment(Qt::AlignCenter); + rate_combo_ = new QComboBox(); + rate_combo_->addItems({"0.01", "0.1", "0.5", "1.0", "2.0", "5.0", "10.0"}); + rate_combo_->setCurrentText(QString("1.0")); + time_label_ = new QLabel("X real time "); + rate_label_->setAlignment(Qt::AlignCenter); + } + + auto * layout = new QHBoxLayout(); + layout->addWidget(pause_button_); + layout->addWidget(apply_rate_button_); + layout->addWidget(rate_label_); + layout->addWidget(rate_combo_); + layout->addWidget(time_label_); + setLayout(layout); + + connect(pause_button_, SIGNAL(clicked()), this, SLOT(onPauseClicked())); + connect(apply_rate_button_, SIGNAL(clicked()), this, SLOT(onApplyRateClicked())); + connect(rate_combo_, SIGNAL(currentIndexChanged(int)), this, SLOT(onRateChanged())); +} + +void BagTimeManagerPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + client_pause_ = + raw_node_->create_client("/rosbag2_player/pause", rmw_qos_profile_services_default); + client_resume_ = + raw_node_->create_client("/rosbag2_player/resume", rmw_qos_profile_services_default); + client_set_rate_ = + raw_node_->create_client("/rosbag2_player/set_rate", rmw_qos_profile_services_default); +} + +void BagTimeManagerPanel::onPauseClicked() +{ + if (current_state_ == STATE::PAUSE) { + // do resume + current_state_ = STATE::RESUME; + pause_button_->setText(QString::fromStdString("Resume")); + // green + pause_button_->setStyleSheet("background-color: #00FF00;"); + auto req = std::make_shared(); + client_resume_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + } else { + // do pause + current_state_ = STATE::PAUSE; + pause_button_->setText(QString::fromStdString("Pause")); + // red + pause_button_->setStyleSheet("background-color: #FF0000;"); + auto req = std::make_shared(); + client_pause_->async_send_request( + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + } +} + +void BagTimeManagerPanel::onApplyRateClicked() +{ + auto request = std::make_shared(); + request->rate = std::stod(rate_combo_->currentText().toStdString()); + client_set_rate_->async_send_request( + request, [this, request](rclcpp::Client::SharedFuture result) { + const auto & response = result.get(); + if (response->success) { + RCLCPP_INFO(raw_node_->get_logger(), "set ros bag rate %f x real time", request->rate); + } else { + RCLCPP_WARN(raw_node_->get_logger(), "service failed"); + } + }); +} +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::BagTimeManagerPanel, rviz_common::Panel) diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp new file mode 100644 index 0000000000000..2cc4b98a6dabf --- /dev/null +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp @@ -0,0 +1,72 @@ +// +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef BAG_TIME_MANAGER_PANEL_HPP_ +#define BAG_TIME_MANAGER_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rviz_plugins +{ +using rosbag2_interfaces::srv::Pause; +using rosbag2_interfaces::srv::Resume; +using rosbag2_interfaces::srv::SetRate; +class BagTimeManagerPanel : public rviz_common::Panel +{ + Q_OBJECT +public: + explicit BagTimeManagerPanel(QWidget * parent = nullptr); + void onInitialize() override; + +protected Q_SLOTS: + /// @brief callback for when the publishing rate is changed + void onRateChanged() {} + /// @brief callback for when the step button is clicked + void onPauseClicked(); + void onApplyRateClicked(); + +protected: + // ROS + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Client::SharedPtr client_pause_; + rclcpp::Client::SharedPtr client_resume_; + rclcpp::Client::SharedPtr client_set_rate_; + + // GUI + QPushButton * pause_button_; + QPushButton * apply_rate_button_; + QLabel * rate_label_; + QLabel * time_label_; + QComboBox * rate_combo_; + +private: + enum STATE { PAUSE, RESUME }; + STATE current_state_{RESUME}; +}; + +} // namespace rviz_plugins + +#endif // BAG_TIME_MANAGER_PANEL_HPP_ diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index 2f59543e2442c..b4a98d90c74e4 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -45,7 +45,7 @@ struct Output class GoalDistanceCalculator { public: - Output update(const Input & input); + static Output update(const Input & input); void setParam(const Param & param) { param_ = param; } diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 10aba5872b66a..50051d928b6c1 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -33,8 +33,8 @@ namespace goal_distance_calculator { struct NodeParam { - double update_rate; - bool oneshot; + double update_rate{0.0}; + bool oneshot{false}; }; class GoalDistanceCalculatorNode : public rclcpp::Node diff --git a/common/interpolation/CMakeLists.txt b/common/interpolation/CMakeLists.txt index 7afed6fe57c41..cc91bed012432 100644 --- a/common/interpolation/CMakeLists.txt +++ b/common/interpolation/CMakeLists.txt @@ -8,6 +8,7 @@ ament_auto_add_library(interpolation SHARED src/linear_interpolation.cpp src/spline_interpolation.cpp src/spline_interpolation_points_2d.cpp + src/spherical_linear_interpolation.cpp ) if(BUILD_TESTING) diff --git a/common/interpolation/README.md b/common/interpolation/README.md index a56af1e864e32..5801c06dcb717 100644 --- a/common/interpolation/README.md +++ b/common/interpolation/README.md @@ -12,7 +12,7 @@ Then it calculates interpolated values on y-axis for `query_keys` on x-axis. ## Spline Interpolation -`slerp(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. +`spline(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. Then it calculates interpolated values on y-axis for `query_keys` on x-axis. ### Evaluation of calculation cost diff --git a/common/interpolation/include/interpolation/interpolation_utils.hpp b/common/interpolation/include/interpolation/interpolation_utils.hpp index d170007685bbf..ed381a3108410 100644 --- a/common/interpolation/include/interpolation/interpolation_utils.hpp +++ b/common/interpolation/include/interpolation/interpolation_utils.hpp @@ -76,8 +76,9 @@ inline void validateKeys( } } -inline void validateKeysAndValues( - const std::vector & base_keys, const std::vector & base_values) +template +void validateKeysAndValues( + const std::vector & base_keys, const std::vector & base_values) { // when vectors are empty if (base_keys.empty() || base_values.empty()) { diff --git a/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp new file mode 100644 index 0000000000000..516b3ab09e8b7 --- /dev/null +++ b/common/interpolation/include/interpolation/spherical_linear_interpolation.hpp @@ -0,0 +1,44 @@ +// 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 INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#define INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ + +#include "interpolation/interpolation_utils.hpp" + +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace interpolation +{ +geometry_msgs::msg::Quaternion slerp( + const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, + const double ratio); + +std::vector slerp( + const std::vector & base_keys, + const std::vector & base_values, + const std::vector & query_keys); +} // namespace interpolation + +#endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index c56498249e91f..9640bcd6c5120 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -46,10 +46,10 @@ struct MultiSplineCoef }; // static spline interpolation functions -std::vector slerp( +std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); -std::vector slerpByAkima( +std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys); } // namespace interpolation diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index c1f08a6d937ae..d44d16d88dd04 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -21,8 +21,12 @@ namespace interpolation { +std::array, 3> slerp2dFromXY( + const std::vector & base_keys, const std::vector & base_x_values, + const std::vector & base_y_values, const std::vector & query_keys); + template -std::vector slerpYawFromPoints(const std::vector & points); +std::vector splineYawFromPoints(const std::vector & points); } // namespace interpolation // non-static points spline interpolation @@ -66,8 +70,8 @@ class SplineInterpolationPoints2d private: void calcSplineCoefficientsInner(const std::vector & points); - SplineInterpolation slerp_x_; - SplineInterpolation slerp_y_; + SplineInterpolation spline_x_; + SplineInterpolation spline_y_; std::vector base_s_vec_; }; diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/interpolation/include/interpolation/zero_order_hold.hpp index 7de6743ed9d16..e48da814c5740 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/interpolation/include/interpolation/zero_order_hold.hpp @@ -28,23 +28,7 @@ std::vector zero_order_hold( { // throw exception for invalid arguments interpolation_utils::validateKeys(base_keys, query_keys); - - // when vectors are empty - if (base_keys.empty() || base_values.empty()) { - throw std::invalid_argument("Points is empty."); - } - - // when size of vectors are less than 2 - if (base_keys.size() < 2 || base_values.size() < 2) { - throw std::invalid_argument( - "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) + - ", base_values.size() = " + std::to_string(base_values.size())); - } - - // when sizes of indices and values are not same - if (base_keys.size() != base_values.size()) { - throw std::invalid_argument("The size of base_keys and base_values are not the same."); - } + interpolation_utils::validateKeysAndValues(base_keys, base_values); std::vector query_values; size_t closest_segment_idx = 0; diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp new file mode 100644 index 0000000000000..014e9011e2a61 --- /dev/null +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -0,0 +1,60 @@ +// 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 "interpolation/spherical_linear_interpolation.hpp" + +namespace interpolation +{ +geometry_msgs::msg::Quaternion slerp( + const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, + const double ratio) +{ + tf2::Quaternion src_tf; + tf2::Quaternion dst_tf; + tf2::fromMsg(src_quat, src_tf); + tf2::fromMsg(dst_quat, dst_tf); + const auto interpolated_quat = tf2::slerp(src_tf, dst_tf, ratio); + return tf2::toMsg(interpolated_quat); +} + +std::vector slerp( + const std::vector & base_keys, + const std::vector & base_values, + const std::vector & query_keys) +{ + // throw exception for invalid arguments + interpolation_utils::validateKeys(base_keys, query_keys); + interpolation_utils::validateKeysAndValues(base_keys, base_values); + + // calculate linear interpolation + std::vector query_values; + size_t key_index = 0; + for (const auto query_key : query_keys) { + while (base_keys.at(key_index + 1) < query_key) { + ++key_index; + } + + const auto src_quat = base_values.at(key_index); + const auto dst_quat = base_values.at(key_index + 1); + const double ratio = (query_key - base_keys.at(key_index)) / + (base_keys.at(key_index + 1) - base_keys.at(key_index)); + + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); + query_values.push_back(interpolated_quat); + } + + return query_values; +} + +} // namespace interpolation diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index ebf7c1ddb18bd..cf00452f1d850 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -81,7 +81,7 @@ inline std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma namespace interpolation { -std::vector slerp( +std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { @@ -94,7 +94,7 @@ std::vector slerp( return interpolator.getSplineInterpolatedValues(query_keys); } -std::vector slerpByAkima( +std::vector splineByAkima( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 2a34e53fc884e..71e2629044f11 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -68,8 +68,31 @@ std::array, 3> getBaseValues( namespace interpolation { + +std::array, 3> slerp2dFromXY( + const std::vector & base_keys, const std::vector & base_x_values, + const std::vector & base_y_values, const std::vector & query_keys) +{ + SplineInterpolation interpolator_x, interpolator_y; + std::vector yaw_vec; + + // calculate spline coefficients + interpolator_x.calcSplineCoefficients(base_keys, base_x_values); + interpolator_y.calcSplineCoefficients(base_keys, base_y_values); + const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys); + const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys); + for (size_t i = 0; i < diff_x.size(); i++) { + double yaw = std::atan2(diff_y[i], diff_x[i]); + yaw_vec.push_back(yaw); + } + // interpolate base_keys at query_keys + return { + interpolator_x.getSplineInterpolatedValues(query_keys), + interpolator_y.getSplineInterpolatedValues(query_keys), yaw_vec}; +} + template -std::vector slerpYawFromPoints(const std::vector & points) +std::vector splineYawFromPoints(const std::vector & points) { SplineInterpolationPoints2d interpolator; @@ -84,8 +107,9 @@ std::vector slerpYawFromPoints(const std::vector & points) } return yaw_vec; } -template std::vector slerpYawFromPoints( +template std::vector splineYawFromPoints( const std::vector & points); + } // namespace interpolation geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( @@ -103,8 +127,8 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin whole_s = base_s_vec_.back(); } - const double x = slerp_x_.getSplineInterpolatedValues({whole_s}).at(0); - const double y = slerp_y_.getSplineInterpolatedValues({whole_s}).at(0); + const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0); + const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0); geometry_msgs::msg::Point geom_point; geom_point.x = x; @@ -126,8 +150,8 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c whole_s = base_s_vec_.back(); } - const double diff_x = slerp_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); - const double diff_y = slerp_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); return std::atan2(diff_y, diff_x); } @@ -150,6 +174,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( const auto & base_y_vec = base.at(2); // calculate spline coefficients - slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); - slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); + spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); + spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); } diff --git a/common/interpolation/test/src/test_spherical_linear_interpolation.cpp b/common/interpolation/test/src/test_spherical_linear_interpolation.cpp new file mode 100644 index 0000000000000..0bb9ba8ef2ce8 --- /dev/null +++ b/common/interpolation/test/src/test_spherical_linear_interpolation.cpp @@ -0,0 +1,137 @@ +// 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 "interpolation/spherical_linear_interpolation.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +namespace +{ +inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( + const double roll, const double pitch, const double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} +} // namespace + +TEST(slerp, spline_scalar) +{ + using interpolation::slerp; + + // Same value + { + const double src_yaw = 0.0; + const double dst_yaw = 0.0; + const auto src_quat = createQuaternionFromRPY(0.0, 0.0, src_yaw); + const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw); + + const auto ans_quat = createQuaternionFromRPY(0.0, 0.0, 0.0); + + for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } + + // Random Value + { + const double src_yaw = 0.0; + const double dst_yaw = M_PI; + const auto src_quat = createQuaternionFromRPY(0.0, 0.0, src_yaw); + const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw); + + for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); + + const double ans_yaw = M_PI * ratio; + tf2::Quaternion ans; + ans.setRPY(0, 0, ans_yaw); + const geometry_msgs::msg::Quaternion ans_quat = tf2::toMsg(ans); + + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } +} + +TEST(slerp, spline_vector) +{ + using interpolation::slerp; + + // query keys are same as base keys + { + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector base_values; + for (size_t i = 0; i < 5; ++i) { + const auto quat = createQuaternionFromRPY(0.0, 0.0, i * M_PI / 5.0); + base_values.push_back(quat); + } + const std::vector query_keys = base_keys; + const auto ans = base_values; + + const auto results = slerp(base_keys, base_values, query_keys); + + for (size_t i = 0; i < results.size(); ++i) { + const auto interpolated_quat = results.at(i); + const auto ans_quat = ans.at(i); + + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } + + // random + { + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector base_values; + for (size_t i = 0; i < 5; ++i) { + const auto quat = createQuaternionFromRPY(0.0, 0.0, i * M_PI / 5.0); + base_values.push_back(quat); + } + const std::vector query_keys = {0.0, 0.1, 1.5, 2.6, 3.1, 3.8}; + std::vector ans(query_keys.size()); + ans.at(0) = createQuaternionFromRPY(0.0, 0.0, 0.0); + ans.at(1) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0); + ans.at(2) = createQuaternionFromRPY(0.0, 0.0, 0.5 * M_PI / 5.0 + M_PI / 5.0); + ans.at(3) = createQuaternionFromRPY(0.0, 0.0, 0.6 * M_PI / 5.0 + 2.0 * M_PI / 5.0); + ans.at(4) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0 + 3.0 * M_PI / 5.0); + ans.at(5) = createQuaternionFromRPY(0.0, 0.0, 0.8 * M_PI / 5.0 + 3.0 * M_PI / 5.0); + + const auto results = slerp(base_keys, base_values, query_keys); + + for (size_t i = 0; i < results.size(); ++i) { + const auto interpolated_quat = results.at(i); + const auto ans_quat = ans.at(i); + + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } +} diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index 132b0100ff3b5..c2bb8eb177ba1 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -23,7 +23,7 @@ constexpr double epsilon = 1e-6; -TEST(spline_interpolation, slerp) +TEST(spline_interpolation, spline) { { // straight: query_keys is same as base_keys const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; @@ -31,7 +31,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -43,7 +43,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -55,7 +55,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -67,7 +67,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.075611, 0.997242, 1.573258}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -79,7 +79,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -91,7 +91,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -103,7 +103,7 @@ TEST(spline_interpolation, slerp) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.808769, -0.077539, 1.035096}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -115,14 +115,14 @@ TEST(spline_interpolation, slerp) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 137.591789, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + const auto query_values = interpolation::spline(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } } -TEST(spline_interpolation, slerpByAkima) +TEST(spline_interpolation, splineByAkima) { { // straight: query_keys is same as base_keys const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; @@ -130,7 +130,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -142,7 +142,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; const std::vector ans{0.0, 1.05, 2.85, 6.0}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -154,7 +154,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -166,7 +166,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.0801, 1.110749, 1.4864}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -178,7 +178,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -190,7 +190,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = base_keys; const std::vector ans = base_values; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -202,7 +202,7 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys{-1.0, 0.0, 4.0}; const std::vector ans{-0.8378, -0.0801, 0.927031}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } @@ -214,14 +214,14 @@ TEST(spline_interpolation, slerpByAkima) const std::vector query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0}; const std::vector ans = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1}; - const auto query_values = interpolation::slerpByAkima(base_keys, base_values, query_keys); + const auto query_values = interpolation::splineByAkima(base_keys, base_values, query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } } -TEST(spline_interpolation, slerpYawFromPoints) +TEST(spline_interpolation, splineYawFromPoints) { using tier4_autoware_utils::createPoint; @@ -235,7 +235,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -251,7 +251,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -261,7 +261,7 @@ TEST(spline_interpolation, slerpYawFromPoints) std::vector points; points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(interpolation::slerpYawFromPoints(points), std::logic_error); + EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) @@ -271,7 +271,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } @@ -285,7 +285,7 @@ TEST(spline_interpolation, slerpYawFromPoints) const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - const auto yaws = interpolation::slerpYawFromPoints(points); + const auto yaws = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < yaws.size(); ++i) { EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); } diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index 6771e3c1a3238..84887dc4e5804 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -4,7 +4,7 @@ kalman_filter 0.1.0 The kalman filter package - Yukihiro Saito Horibe + Yukihiro Saito Apache License 2.0 Takamasa Horibe diff --git a/common/motion_common/include/motion_common/motion_common.hpp b/common/motion_common/include/motion_common/motion_common.hpp index e73cff3ed6e3f..2742b1c073bda 100644 --- a/common/motion_common/include/motion_common/motion_common.hpp +++ b/common/motion_common/include/motion_common/motion_common.hpp @@ -159,11 +159,11 @@ T interpolate(T a, T b, RealT t) } /// Spherical linear interpolation -MOTION_COMMON_PUBLIC Orientation slerp(const Orientation & a, const Orientation & b, const Real t); +MOTION_COMMON_PUBLIC Orientation spline(const Orientation & a, const Orientation & b, const Real t); /// Trajectory point interpolation template -Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn) +Point interpolate(Point a, Point b, Real t, SlerpF spline_fn) { Point ret{rosidl_runtime_cpp::MessageInitialization::ALL}; { @@ -173,7 +173,7 @@ Point interpolate(Point a, Point b, Real t, SlerpF slerp_fn) } ret.pose.position.x = interpolate(a.pose.position.x, b.pose.position.x, t); ret.pose.position.y = interpolate(a.pose.position.y, b.pose.position.y, t); - ret.pose.orientation = slerp_fn(a.pose.orientation, b.pose.orientation, t); + ret.pose.orientation = spline_fn(a.pose.orientation, b.pose.orientation, t); ret.longitudinal_velocity_mps = interpolate(a.longitudinal_velocity_mps, b.longitudinal_velocity_mps, t); ret.lateral_velocity_mps = interpolate(a.lateral_velocity_mps, b.lateral_velocity_mps, t); @@ -191,7 +191,7 @@ MOTION_COMMON_PUBLIC Point interpolate(Point a, Point b, Real t); /// Sample a trajectory using interpolation; does not extrapolate template void sample( - const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period, SlerpF slerp_fn) + const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period, SlerpF spline_fn) { out.points.clear(); out.header = in.header; @@ -230,7 +230,7 @@ void sample( const auto dt_ = std::chrono::duration_cast>( ref_duration - from_message(curr_pt.time_from_start)); const auto t = dt_.count() / dt.count(); - out.points.push_back(interpolate(curr_pt, next_pt, t, slerp_fn)); + out.points.push_back(interpolate(curr_pt, next_pt, t, spline_fn)); } ref_duration += period; } diff --git a/common/motion_common/src/motion_common/motion_common.cpp b/common/motion_common/src/motion_common/motion_common.cpp index aaedabfd83b9a..7ad71096c53f8 100644 --- a/common/motion_common/src/motion_common/motion_common.cpp +++ b/common/motion_common/src/motion_common/motion_common.cpp @@ -153,7 +153,7 @@ Heading nlerp(Heading a, Heading b, Real t) } //////////////////////////////////////////////////////////////////////////////// -Orientation slerp(const Orientation & a, const Orientation & b, const Real t) +Orientation spline(const Orientation & a, const Orientation & b, const Real t) { tf2::Quaternion quat_a; tf2::Quaternion quat_b; @@ -163,12 +163,12 @@ Orientation slerp(const Orientation & a, const Orientation & b, const Real t) } //////////////////////////////////////////////////////////////////////////////// -Point interpolate(Point a, Point b, Real t) { return interpolate(a, b, t, slerp); } +Point interpolate(Point a, Point b, Real t) { return interpolate(a, b, t, spline); } //////////////////////////////////////////////////////////////////////////////// void sample(const Trajectory & in, Trajectory & out, std::chrono::nanoseconds period) { - sample(in, out, period, slerp); + sample(in, out, period, spline); } //////////////////////////////////////////////////////////////////////////////// diff --git a/common/motion_common/test/interpolation.cpp b/common/motion_common/test/interpolation.cpp index e59d9e0af9346..ae57e5ea76ba6 100644 --- a/common/motion_common/test/interpolation.cpp +++ b/common/motion_common/test/interpolation.cpp @@ -109,8 +109,8 @@ TEST(Interpolation, Slerp2d) }; // Plain check const auto test_case = [=](Orientation a, Orientation b, Real t, Orientation res, Real tol) { - using motion::motion_common::slerp; - const auto ret = slerp(a, b, t); + using motion::motion_common::spline; + const auto ret = spline(a, b, t); EXPECT_LT(std::fabs(to_angle(ret) - to_angle(res)), tol) << to_angle(ret) << ", " << to_angle(res); }; @@ -123,10 +123,10 @@ TEST(Interpolation, Slerp2d) const auto t_ = motion::motion_common::clamp(t, 0.0F, 1.0F); const auto th_t = th_a + (t_ * ab); const auto res_th = from_angle(th_t); - using motion::motion_common::slerp; + using motion::motion_common::spline; test_case(a, b, t, res_th, tol); if (HasFailure()) { - const auto ret = slerp(a, b, t); + const auto ret = spline(a, b, t); std::cout << "Angles: " << th_a << ", " << th_b << "; " << th_t << ", " << to_angle(ret) << "\n"; } diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index 59e43d2ec0347..941855eb62528 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(motion_utils SHARED src/motion_utils.cpp src/marker/marker_helper.cpp src/resample/resample.cpp + src/trajectory/interpolation.cpp src/vehicle/vehicle_state_checker.cpp ) diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp new file mode 100644 index 0000000000000..b05a5334b7328 --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -0,0 +1,49 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#define MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/constants.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include + +#include +#include +#include +#include + +namespace motion_utils +{ +/** + * @brief An interpolation function that finds the closest interpolated point on the trajectory from + * the given pose + * @param trajectory input trajectory + * @param target_pose target_pose + * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for + * twist information + * @return resampled path(poses) + */ +autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +} // namespace motion_utils + +#endif // MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index a10db47a377a2..a4a3842d1047d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -20,13 +20,14 @@ #include +#include #include #include namespace motion_utils { inline boost::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -35,7 +36,7 @@ inline boost::optional> getPathIndexRangeWithLaneId( for (size_t i = 0; i < path.points.size(); ++i) { const auto & p = path.points.at(i); for (const auto & id : p.lane_ids) { - if (id == lane_id) { + if (id == target_lane_id) { if (!found_first_idx) { start_idx = i; found_first_idx = true; @@ -46,6 +47,10 @@ inline boost::optional> getPathIndexRangeWithLaneId( } if (found_first_idx) { + // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. + start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); + end_idx = std::min(path.points.size() - 1, end_idx + 1); + return std::make_pair(start_idx, end_idx); } diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index affe832f6b221..7f050a4454ffe 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -66,13 +66,13 @@ std::vector resamplePath( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; - const auto slerp = [&](const auto & input) { - return interpolation::slerp(input_arclength, input, resampled_arclength); + const auto spline = [&](const auto & input) { + return interpolation::spline(input_arclength, input, resampled_arclength); }; - const auto interpolated_x = use_lerp_for_xy ? lerp(x) : slerp(x); - const auto interpolated_y = use_lerp_for_xy ? lerp(y) : slerp(y); - const auto interpolated_z = use_lerp_for_z ? lerp(z) : slerp(z); + const auto interpolated_x = use_lerp_for_xy ? lerp(x) : spline(x); + const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y); + const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); std::vector resampled_points; resampled_points.resize(interpolated_x.size()); diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp new file mode 100644 index 0000000000000..cd603676ddd78 --- /dev/null +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -0,0 +1,92 @@ +// 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 "motion_utils/trajectory/interpolation.hpp" + +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/zero_order_hold.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +namespace motion_utils +{ +TrajectoryPoint calcInterpolatedPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, + const bool use_zero_order_hold_for_twist, const double dist_threshold, const double yaw_threshold) +{ + if (trajectory.points.empty()) { + TrajectoryPoint interpolated_point{}; + interpolated_point.pose = target_pose; + return interpolated_point; + } else if (trajectory.points.size() == 1) { + return trajectory.points.front(); + } + + const size_t segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory.points, target_pose, dist_threshold, yaw_threshold); + + // Calculate interpolation ratio + const auto & curr_pt = trajectory.points.at(segment_idx); + const auto & next_pt = trajectory.points.at(segment_idx + 1); + const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_pose); + if (v1.length2() < 1e-3) { + return curr_pt; + } + + const double ratio = v1.dot(v2) / v1.length2(); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + // Interpolate + TrajectoryPoint interpolated_point{}; + + // pose interpolation + interpolated_point.pose = + tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + + // twist interpolation + if (use_zero_order_hold_for_twist) { + interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; + interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; + interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; + } else { + interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); + interpolated_point.lateral_velocity_mps = interpolation::lerp( + curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); + interpolated_point.acceleration_mps2 = + interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + } + + // heading rate interpolation + interpolated_point.heading_rate_rps = + interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + + // wheel interpolation + interpolated_point.front_wheel_angle_rad = interpolation::lerp( + curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = + interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + + // time interpolation + const double interpolated_time = interpolation::lerp( + rclcpp::Duration(curr_pt.time_from_start).seconds(), + rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); + interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); + + return interpolated_point; +} +} // namespace motion_utils diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp new file mode 100644 index 0000000000000..f0e45179d05b6 --- /dev/null +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -0,0 +1,309 @@ +// 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 "motion_utils/trajectory/interpolation.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" + +#include +#include +#include + +#include +#include + +namespace +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createQuaternionFromRPY; +using tier4_autoware_utils::transformPoint; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +TrajectoryPoint generateTestTrajectoryPoint( + const double x, const double y, const double z, const double theta = 0.0, + const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0, + const double acc = 0.0) +{ + TrajectoryPoint p; + p.pose = createPose(x, y, z, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate; + p.acceleration_mps2 = acc; + return p; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel_lon = 0.0, + const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double acc = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate_rps; + p.acceleration_mps2 = acc; + traj.points.push_back(p); + } + return traj; +} +} // namespace + +TEST(Interpolation, interpolate_path_for_trajectory) +{ + using motion_utils::calcInterpolatedPoint; + + { + autoware_auto_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + // Same points as the trajectory point + { + const auto target_pose = createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Random Point + { + const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.25, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.125, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Random Point with zero order hold + { + const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose, true); + + EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 2.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.1, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Initial Point + { + const auto target_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Terminal Point + { + const auto target_pose = createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Outside of initial point + { + const auto target_pose = createPose(-2.0, -9.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Outside of terminal point + { + const auto target_pose = createPose(10.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + } + + // Empty Point + { + const Trajectory traj; + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 5.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // One point + { + const auto traj = generateTestTrajectory(1, 1.0, 1.0, 0.5, 0.1, 0.05); + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.1, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.05, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Duplicated Points + { + autoware_auto_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(4) = traj.points.at(3); + + const auto target_pose = createPose(3.2, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } +} diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 1eadf7105d087..d3e2565c99f1b 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -72,16 +72,16 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { const auto res = getPathIndexRangeWithLaneId(points, 3); EXPECT_EQ(res->first, 0U); - EXPECT_EQ(res->second, 1U); + EXPECT_EQ(res->second, 2U); } { const auto res = getPathIndexRangeWithLaneId(points, 1); - EXPECT_EQ(res->first, 2U); - EXPECT_EQ(res->second, 2U); + EXPECT_EQ(res->first, 1U); + EXPECT_EQ(res->second, 3U); } { const auto res = getPathIndexRangeWithLaneId(points, 2); - EXPECT_EQ(res->first, 3U); + EXPECT_EQ(res->first, 2U); EXPECT_EQ(res->second, 5U); } { @@ -142,7 +142,7 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId) for (size_t i = 3; i < 9; ++i) { modified_path.points.at(i).lane_ids = {100}; } - EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 3U); + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); EXPECT_EQ( findNearestSegmentIndexFromLaneId(modified_path, createPoint(2.4, 1.3, 0.0), 100), 2U); } diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/osqp_interface/design/osqp_interface-design.md index 8fd6ab28a659d..e9ff4a2a3bc3a 100644 --- a/common/osqp_interface/design/osqp_interface-design.md +++ b/common/osqp_interface/design/osqp_interface-design.md @@ -68,5 +68,3 @@ The interface can be used in several ways: ## Related issues - -- This package was introduced as a dependency of the MPC-based lateral controller: diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt index 46f7ae74774e2..b6d9376a790ae 100644 --- a/common/perception_utils/CMakeLists.txt +++ b/common/perception_utils/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(Boost REQUIRED) ament_auto_add_library(perception_utils SHARED src/perception_utils.cpp + src/predicted_path_utils.cpp ) if(BUILD_TESTING) diff --git a/common/perception_utils/include/perception_utils/predicted_path_utils.hpp b/common/perception_utils/include/perception_utils/predicted_path_utils.hpp index 314318ca16b12..4e5593bff3412 100644 --- a/common/perception_utils/include/perception_utils/predicted_path_utils.hpp +++ b/common/perception_utils/include/perception_utils/predicted_path_utils.hpp @@ -16,6 +16,7 @@ #define PERCEPTION_UTILS__PREDICTED_PATH_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" +#include "interpolation/spherical_linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "perception_utils/geometry.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -39,28 +40,8 @@ namespace perception_utils * time_step*(num_of_path_points)] * @return interpolated pose */ -inline boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) -{ - // Check if relative time is in the valid range - if (path.path.empty() || relative_time < 0.0) { - return boost::none; - } - - constexpr double epsilon = 1e-6; - const double & time_step = rclcpp::Duration(path.time_step).seconds(); - for (size_t path_idx = 1; path_idx < path.path.size(); ++path_idx) { - const auto & pt = path.path.at(path_idx); - const auto & prev_pt = path.path.at(path_idx - 1); - if (relative_time - epsilon < time_step * path_idx) { - const double offset = relative_time - time_step * (path_idx - 1); - const double ratio = std::clamp(offset / time_step, 0.0, 1.0); - return tier4_autoware_utils::calcInterpolatedPose(prev_pt, pt, ratio); - } - } - - return boost::none; -} +boost::optional calcInterpolatedPose( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time); /** * @brief Resampling predicted path by time step vector. Note this function does not substitute @@ -70,63 +51,10 @@ inline boost::optional calcInterpolatedPose( * time_step*(num_of_path_points)] * @return resampled path */ -inline autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const autoware_auto_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy = true, - const bool use_spline_for_z = false) -{ - if (path.path.empty() || resampled_time.empty()) { - throw std::invalid_argument("input path or resampled_time is empty"); - } - - const double & time_step = rclcpp::Duration(path.time_step).seconds(); - std::vector input_time(path.path.size()); - std::vector x(path.path.size()); - std::vector y(path.path.size()); - std::vector z(path.path.size()); - for (size_t i = 0; i < path.path.size(); ++i) { - input_time.at(i) = time_step * i; - x.at(i) = path.path.at(i).position.x; - y.at(i) = path.path.at(i).position.y; - z.at(i) = path.path.at(i).position.z; - } - - const auto lerp = [&](const auto & input) { - return interpolation::lerp(input_time, input, resampled_time); - }; - const auto slerp = [&](const auto & input) { - return interpolation::slerp(input_time, input, resampled_time); - }; - - const auto interpolated_x = use_spline_for_xy ? slerp(x) : lerp(x); - const auto interpolated_y = use_spline_for_xy ? slerp(y) : lerp(y); - const auto interpolated_z = use_spline_for_z ? slerp(z) : lerp(z); - - autoware_auto_perception_msgs::msg::PredictedPath resampled_path; - const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); - resampled_path.confidence = path.confidence; - resampled_path.path.resize(resampled_size); - - // Set Position - for (size_t i = 0; i < resampled_size; ++i) { - const auto p = tier4_autoware_utils::createPoint( - interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); - resampled_path.path.at(i).position = p; - } - - // Set Quaternion - for (size_t i = 0; i < resampled_size - 1; ++i) { - const auto & src_point = resampled_path.path.at(i).position; - const auto & dst_point = resampled_path.path.at(i + 1).position; - const double pitch = tier4_autoware_utils::calcElevationAngle(src_point, dst_point); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); - resampled_path.path.at(i).orientation = - tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); - } - resampled_path.path.back().orientation = resampled_path.path.at(resampled_size - 2).orientation; - - return resampled_path; -} + const bool use_spline_for_z = false); /** * @brief Resampling predicted path by sampling time interval. Note that this function samples @@ -136,37 +64,10 @@ inline autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( * @param sampling_horizon sampling time horizon * @return resampled path */ -inline autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const autoware_auto_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, const double sampling_horizon, - const bool use_spline_for_xy = true, const bool use_spline_for_z = false) -{ - if (path.path.empty()) { - throw std::invalid_argument("Predicted Path is empty"); - } - - if (sampling_time_interval <= 0.0 || sampling_horizon <= 0.0) { - throw std::invalid_argument("sampling time interval or sampling time horizon is negative"); - } - - // Calculate Horizon - const double predicted_horizon = - rclcpp::Duration(path.time_step).seconds() * static_cast(path.path.size() - 1); - const double horizon = std::min(predicted_horizon, sampling_horizon); - - // Get sampling time vector - constexpr double epsilon = 1e-6; - std::vector sampling_time_vector; - for (double t = 0.0; t < horizon + epsilon; t += sampling_time_interval) { - sampling_time_vector.push_back(t); - } - - // Resample and substitute time interval - auto resampled_path = - resamplePredictedPath(path, sampling_time_vector, use_spline_for_xy, use_spline_for_z); - resampled_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval); - return resampled_path; -} + const bool use_spline_for_xy = true, const bool use_spline_for_z = false); } // namespace perception_utils #endif // PERCEPTION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/perception_utils/src/predicted_path_utils.cpp b/common/perception_utils/src/predicted_path_utils.cpp new file mode 100644 index 0000000000000..96ca74d2cb234 --- /dev/null +++ b/common/perception_utils/src/predicted_path_utils.cpp @@ -0,0 +1,127 @@ +// 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_utils/predicted_path_utils.hpp" + +namespace perception_utils +{ +boost::optional calcInterpolatedPose( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) +{ + // Check if relative time is in the valid range + if (path.path.empty() || relative_time < 0.0) { + return boost::none; + } + + constexpr double epsilon = 1e-6; + const double & time_step = rclcpp::Duration(path.time_step).seconds(); + for (size_t path_idx = 1; path_idx < path.path.size(); ++path_idx) { + const auto & pt = path.path.at(path_idx); + const auto & prev_pt = path.path.at(path_idx - 1); + if (relative_time - epsilon < time_step * path_idx) { + const double offset = relative_time - time_step * (path_idx - 1); + const double ratio = std::clamp(offset / time_step, 0.0, 1.0); + return tier4_autoware_utils::calcInterpolatedPose(prev_pt, pt, ratio, false); + } + } + + return boost::none; +} + +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const std::vector & resampled_time, const bool use_spline_for_xy, + const bool use_spline_for_z) +{ + if (path.path.empty() || resampled_time.empty()) { + throw std::invalid_argument("input path or resampled_time is empty"); + } + + const double & time_step = rclcpp::Duration(path.time_step).seconds(); + std::vector input_time(path.path.size()); + std::vector x(path.path.size()); + std::vector y(path.path.size()); + std::vector z(path.path.size()); + std::vector quat(path.path.size()); + for (size_t i = 0; i < path.path.size(); ++i) { + input_time.at(i) = time_step * i; + x.at(i) = path.path.at(i).position.x; + y.at(i) = path.path.at(i).position.y; + z.at(i) = path.path.at(i).position.z; + quat.at(i) = path.path.at(i).orientation; + } + + const auto lerp = [&](const auto & input) { + return interpolation::lerp(input_time, input, resampled_time); + }; + const auto spline = [&](const auto & input) { + return interpolation::spline(input_time, input, resampled_time); + }; + const auto slerp = [&](const auto & input) { + return interpolation::slerp(input_time, input, resampled_time); + }; + + const auto interpolated_x = use_spline_for_xy ? spline(x) : lerp(x); + const auto interpolated_y = use_spline_for_xy ? spline(y) : lerp(y); + const auto interpolated_z = use_spline_for_z ? spline(z) : lerp(z); + const auto interpolated_quat = slerp(quat); + + autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); + resampled_path.confidence = path.confidence; + resampled_path.path.resize(resampled_size); + + // Set Position + for (size_t i = 0; i < resampled_size; ++i) { + const auto p = tier4_autoware_utils::createPoint( + interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); + resampled_path.path.at(i).position = p; + resampled_path.path.at(i).orientation = interpolated_quat.at(i); + } + + return resampled_path; +} + +autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_auto_perception_msgs::msg::PredictedPath & path, + const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy, + const bool use_spline_for_z) +{ + if (path.path.empty()) { + throw std::invalid_argument("Predicted Path is empty"); + } + + if (sampling_time_interval <= 0.0 || sampling_horizon <= 0.0) { + throw std::invalid_argument("sampling time interval or sampling time horizon is negative"); + } + + // Calculate Horizon + const double predicted_horizon = + rclcpp::Duration(path.time_step).seconds() * static_cast(path.path.size() - 1); + const double horizon = std::min(predicted_horizon, sampling_horizon); + + // Get sampling time vector + constexpr double epsilon = 1e-6; + std::vector sampling_time_vector; + for (double t = 0.0; t < horizon + epsilon; t += sampling_time_interval) { + sampling_time_vector.push_back(t); + } + + // Resample and substitute time interval + auto resampled_path = + resamplePredictedPath(path, sampling_time_vector, use_spline_for_xy, use_spline_for_z); + resampled_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval); + return resampled_path; +} +} // namespace perception_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 835f0c37d3031..8bbfe681fa3ae 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -18,7 +18,6 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -// #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -35,6 +34,7 @@ geometry_msgs::msg::Polygon rotatePolygon( const geometry_msgs::msg::Polygon & polygon, const double & angle); Polygon2d toPolygon2d( const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 115b544726bdf..2ba61fe321cbe 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -430,6 +430,18 @@ inline geometry_msgs::msg::TransformStamped pose2transform( return transform; } +template +tf2::Vector3 point2tfVector(const Point1 & src, const Point2 & dst) +{ + const auto src_p = getPoint(src); + const auto dst_p = getPoint(dst); + + double dx = dst_p.x - src_p.x; + double dy = dst_p.y - src_p.y; + double dz = dst_p.z - src_p.z; + return tf2::Vector3(dx, dy, dz); +} + inline Point3d transformPoint( const Point3d & point, const geometry_msgs::msg::Transform & transform) { diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 193cf657ea830..1e9acb6101935 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -4,7 +4,7 @@ tier4_autoware_utils 0.1.0 The tier4_autoware_utils package - Takamasa Horibe + Takamasa Horibe Kenji Miyake Apache License 2.0 diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index f9ba0f537f07f..b54f888ad0ae1 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -167,6 +167,13 @@ Polygon2d toPolygon2d( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } +tier4_autoware_utils::Polygon2d toPolygon2d( + const autoware_auto_perception_msgs::msg::PredictedObject & object) +{ + return tier4_autoware_utils::toPolygon2d( + object.kinematics.initial_pose_with_covariance.pose, object.shape); +} + double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) { if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index 6b8d886b5196a..70ebb8ac72dca 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -752,6 +752,73 @@ TEST(geometry, pose2transform) } } +TEST(geometry, point2tfVector) +{ + using tier4_autoware_utils::createQuaternionFromRPY; + using tier4_autoware_utils::deg2rad; + using tier4_autoware_utils::point2tfVector; + + // Point + { + geometry_msgs::msg::Point src; + src.x = 1.0; + src.y = 2.0; + src.z = 3.0; + + geometry_msgs::msg::Point dst; + dst.x = 10.0; + dst.y = 5.0; + dst.z = -5.0; + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } + + // Pose + { + geometry_msgs::msg::Pose src; + src.position.x = 1.0; + src.position.y = 2.0; + src.position.z = 3.0; + src.orientation = createQuaternionFromRPY(deg2rad(30), deg2rad(30), deg2rad(30)); + + geometry_msgs::msg::Pose dst; + dst.position.x = 10.0; + dst.position.y = 5.0; + dst.position.z = -5.0; + dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } + + // Point and Pose + { + geometry_msgs::msg::Point src; + src.x = 1.0; + src.y = 2.0; + src.z = 3.0; + + geometry_msgs::msg::Pose dst; + dst.position.x = 10.0; + dst.position.y = 5.0; + dst.position.z = -5.0; + dst.orientation = createQuaternionFromRPY(deg2rad(10), deg2rad(10), deg2rad(10)); + + const auto vec = point2tfVector(src, dst); + + EXPECT_DOUBLE_EQ(vec.x(), 9.0); + EXPECT_DOUBLE_EQ(vec.y(), 3.0); + EXPECT_DOUBLE_EQ(vec.z(), -8.0); + } +} + TEST(geometry, transformPoint) { using tier4_autoware_utils::createQuaternionFromRPY; diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp index 53f54202a9cce..df01ffc73c466 100644 --- a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -35,6 +36,7 @@ #include #include +#include namespace rviz_plugins { @@ -62,6 +64,8 @@ private Q_SLOTS: const QColor & color_min, const QColor & color_max, const double ratio); Ogre::ManualObject * path_manual_object_; Ogre::ManualObject * velocity_manual_object_; + std::vector velocity_texts_; + std::vector velocity_text_nodes_; rviz_common::properties::BoolProperty * property_path_view_; rviz_common::properties::BoolProperty * property_velocity_view_; rviz_common::properties::FloatProperty * property_path_width_; @@ -70,6 +74,8 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_path_alpha_; rviz_common::properties::FloatProperty * property_velocity_alpha_; rviz_common::properties::FloatProperty * property_velocity_scale_; + rviz_common::properties::BoolProperty * property_velocity_text_view_; + rviz_common::properties::FloatProperty * property_velocity_text_scale_; rviz_common::properties::BoolProperty * property_path_color_view_; rviz_common::properties::BoolProperty * property_velocity_color_view_; rviz_common::properties::FloatProperty * property_vel_max_; diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp index 590c7ac9e0320..0ae03ffecce28 100644 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp @@ -85,7 +85,10 @@ AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay() "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); property_velocity_color_ = new rviz_common::properties::ColorProperty( "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - + property_velocity_text_view_ = new rviz_common::properties::BoolProperty( + "View Text Velocity", false, "", this, SLOT(updateVisualization()), this); + property_velocity_text_scale_ = new rviz_common::properties::FloatProperty( + "Scale", 0.3, "", property_velocity_text_view_, SLOT(updateVisualization()), this); property_vel_max_ = new rviz_common::properties::FloatProperty( "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); property_vel_max_->setMin(0.0); @@ -96,6 +99,12 @@ AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() if (initialized()) { scene_manager_->destroyManualObject(path_manual_object_); scene_manager_->destroyManualObject(velocity_manual_object_); + for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->removeAndDestroyAllChildren(); + node->detachAllObjects(); + scene_manager_->destroySceneNode(node); + } } } @@ -168,6 +177,29 @@ void AutowarePathWithLaneIdDisplay::processMessage( // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + if (msg_ptr->points.size() > velocity_texts_.size()) { + for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { + Ogre::SceneNode * node = scene_node_->createChildSceneNode(); + rviz_rendering::MovableText * text = + new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); + text->setVisible(false); + text->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node->attachObject(text); + velocity_texts_.push_back(text); + velocity_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < velocity_texts_.size()) { + for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + scene_manager_->destroySceneNode(node); + } + velocity_texts_.resize(msg_ptr->points.size()); + velocity_text_nodes_.resize(msg_ptr->points.size()); + } + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & e = msg_ptr->points.at(point_idx); /* @@ -236,6 +268,29 @@ void AutowarePathWithLaneIdDisplay::processMessage( e.point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); velocity_manual_object_->colour(color); } + + /* + * Velocity Text + */ + if (property_velocity_text_view_->getBool()) { + Ogre::Vector3 position; + position.x = e.point.pose.position.x; + position.y = e.point.pose.position.y; + position.z = e.point.pose.position.z; + Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + double vel = e.point.longitudinal_velocity_mps; + text->setCaption( + std::to_string(static_cast(std::floor(vel))) + "." + + std::to_string(static_cast(std::floor(vel * 100)))); + text->setCharacterHeight(property_velocity_text_scale_->getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + text->setVisible(false); + } } path_manual_object_->end(); diff --git a/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md b/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md index 88ba349675659..1a83c48a34aed 100644 --- a/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md +++ b/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md @@ -147,5 +147,3 @@ Not Available. ## Related issues - - diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 67626d8de41cb..17f1b35129347 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -9,6 +9,9 @@ Apache License 2.0 + Ali Boyali + Berkay Karaman + ament_cmake_auto rosidl_default_generators diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml index 11d47f8afa3d9..ad992590adc7b 100644 --- a/control/joy_controller/package.xml +++ b/control/joy_controller/package.xml @@ -7,6 +7,8 @@ Kenji Miyake Apache License 2.0 + Kenji Miyake + ament_cmake_auto autoware_cmake diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 6658dcc61bd57..b73105c6b5ee0 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -4,9 +4,11 @@ lane_departure_checker 0.1.0 The lane_departure_checker package - Kenji Miyake + Kyoichi Sugahara Apache License 2.0 + Kenji Miyake + ament_cmake_auto autoware_cmake diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 2c5f980b899be..c49dcf646c105 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -7,6 +7,7 @@ Kenji Miyake Apache License 2.0 + Kenji Miyake Satoshi Tanaka ament_cmake diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index d469750163547..9db703b6ea7db 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -2,9 +2,11 @@ operation_mode_transition_manager 0.1.0 The vehicle_cmd_gate package - Takamasa Horibe + Takamasa Horibe Apache License 2.0 + Takamasa Horibe + autoware_cmake rosidl_default_generators diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index b780835bff3df..0cd68d0749ee0 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -4,10 +4,12 @@ pure_pursuit 0.1.0 The pure_pursuit package - Kenji Miyake - Takamasa Horibe + Takamasa Horibe Apache License 2.0 + Kenji Miyake + Takamasa Horibe + ament_cmake_auto autoware_cmake diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index 29033dcf960bf..a8c347b061472 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -4,10 +4,10 @@ shift_decider 0.1.0 The shift_decider package - Takamasa Horibe + Takamasa Horibe Apache License 2.0 - Takamasa Horibe + Takamasa Horibe ament_cmake diff --git a/control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg b/control/trajectory_follower/design/media/BrakeKeeping.drawio.svg similarity index 100% rename from control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg rename to control/trajectory_follower/design/media/BrakeKeeping.drawio.svg diff --git a/control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg b/control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg similarity index 100% rename from control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg rename to control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg diff --git a/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg b/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg similarity index 100% rename from control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg rename to control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg diff --git a/control/trajectory_follower_nodes/design/lateral_controller-design.md b/control/trajectory_follower/design/mpc_lateral_controller-design.md similarity index 81% rename from control/trajectory_follower_nodes/design/lateral_controller-design.md rename to control/trajectory_follower/design/mpc_lateral_controller-design.md index 450e7d04707d3..ee79cc88f206b 100644 --- a/control/trajectory_follower_nodes/design/lateral_controller-design.md +++ b/control/trajectory_follower/design/mpc_lateral_controller-design.md @@ -1,4 +1,4 @@ -# Lateral Controller +# MPC Lateral Controller This is the design document for the lateral controller node in the `trajectory_follower_nodes` package. @@ -22,29 +22,62 @@ The node uses an implementation of linear model predictive control (MPC) for acc The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. The optimization of the control command is formulated as a Quadratic Program (QP). -These functionalities are implemented in the `trajectory_follower` package -(see [trajectory_follower-mpc-design](../../trajectory_follower/design/trajectory_follower-mpc-design.md#mpc-trajectory-follower)) +Different vehicle models are implemented: -### Assumptions / Known limits +- kinematics : bicycle kinematics model with steering 1st-order delay. +- kinematics_no_delay : bicycle kinematics model without steering delay. +- dynamics : bicycle dynamics model considering slip angle. + The kinematics model is being used by default. Please see the reference [1] for more details. + +For the optimization, a Quadratic Programming (QP) solver is used and two options are currently implemented: + +- unconstraint_fast : use least square method to solve unconstraint QP with eigen. +- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) algorithm (for more details see the related papers at the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): + +### Filtering + +Filtering is required for good noise reduction. +A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for +the output steering angle. +Other filtering methods can be considered as long as the noise reduction performances are good +enough. +The moving average filter for example is not suited and can yield worse results than without any +filtering. + +## Assumptions / Known limits The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose. -- Issue to add points behind ego: - -### Inputs / Outputs / API +## Inputs / Outputs / API -Inputs +### Inputs + +Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_auto_vehicle_msgs/SteeringReport` current steering -- `input/reference_trajectory` : reference trajectory to follow. -- `input/current_kinematic_state`: current state of the vehicle (position, velocity, etc). - Output -- `output/lateral_control_cmd`: generated lateral control command. +### Outputs + +Return LateralOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_control_msgs/AckermannLateralCommand` +- LateralSyncData + - steer angle convergence + +### MPC class + +The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. +Once a vehicle model, a QP solver, and the reference trajectory to follow have been set +(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command +can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. ### Parameter description @@ -66,7 +99,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | | stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | | converged_steer_rad | double | threshold value of the steer convergence | 0.1 | -| keep_steer_control_until_converged | bool | keep steer control until steer is converged | false | +| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | | new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | | new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | @@ -132,8 +165,13 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. - `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. - `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. +## References / External links + + + +- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", + Robotics Institute, Carnegie Mellon University, February 2009. + ## Related issues - -- diff --git a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md b/control/trajectory_follower/design/pid_longitudinal_controller-design.md similarity index 87% rename from control/trajectory_follower_nodes/design/longitudinal_controller-design.md rename to control/trajectory_follower/design/pid_longitudinal_controller-design.md index 207a9755d7117..0cca6803490cd 100644 --- a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md +++ b/control/trajectory_follower/design/pid_longitudinal_controller-design.md @@ -1,4 +1,4 @@ -# Longitudinal Controller +# PID Longitudinal Controller ## Purpose / Use cases @@ -9,56 +9,7 @@ It is assumed that the target acceleration calculated here will be properly real Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface. -## Separated lateral (steering) and longitudinal (velocity) controls - -This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. - -- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. -- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. - -Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. - -### Complex requirements for longitudinal motion - -The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. - -In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. - -There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. - -### Nonlinear coupling of lateral and longitudinal motion - -The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. - -Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. - -## Design - -## Assumptions / Known limits - -1. Smoothed target velocity and its acceleration shall be set in the trajectory - 1. The velocity command is not smoothed inside the controller (only noise may be removed). - 2. For step-like target signal, tracking is performed as fast as possible. -2. The vehicle velocity must be an appropriate value - 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction - 2. The ego-velocity should be given with appropriate noise processing. - 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. -3. The output of this controller must be achieved by later modules (e.g. vehicle interface). - 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. - -## Inputs / Outputs / API - -### Output - -- control_cmd [`autoware_auto_msgs/LongitudinalCommand`] : command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. -- debug_values [`autoware_auto_msgs/Float32MultiArrayDiagnostic`] : debug values used for the control command generation (e.g. the contributions of each P-I-D terms). - -### Input - -- current_state [`autoware_auto_msgs/VehicleKinematicState`] : Current ego state including the current pose and velocity. -- current_trajectory [`autoware_auto_msgs/Trajectory`] : Current target trajectory for the desired velocity on the each trajectory points. - -## Inner-workings / Algorithms +## Design / Inner-workings / Algorithms ### States @@ -141,7 +92,59 @@ Depending on the actuating principle of the vehicle, the mechanism that physical In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. -### Parameter description +### Slope compensation + +Based on the slope information, a compensation term is added to the target acceleration. + +There are two sources of the slope information, which can be switched by a parameter. + +- Pitch of the estimated ego-pose (default) + - Calculates the current slope from the pitch angle of the estimated ego-pose + - Pros: Easily available + - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. +- Z coordinate on the trajectory + - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory + - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained + - Pros: Can be used in combination with delay compensation (not yet implemented) + - Cons: z-coordinates of high-precision map is needed. + - Cons: Does not support free space planning (for now) + +## Assumptions / Known limits + +1. Smoothed target velocity and its acceleration shall be set in the trajectory + 1. The velocity command is not smoothed inside the controller (only noise may be removed). + 2. For step-like target signal, tracking is performed as fast as possible. +2. The vehicle velocity must be an appropriate value + 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction + 2. The ego-velocity should be given with appropriate noise processing. + 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. +3. The output of this controller must be achieved by later modules (e.g. vehicle interface). + 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. + +## Inputs / Outputs / API + +### Input + +Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry + +### Output + +Return LongitudinalOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- LongitudinalSyncData + - velocity convergence(currently not used) + +### PIDController class + +The `PIDController` class is straightforward to use. +First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. +Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. + +## Parameter description The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. @@ -154,7 +157,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | | enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | | enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | -| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | false | +| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | true | | max_acc | double | max value of output acceleration [m/s^2] | 3.0 | | min_acc | double | min value of output acceleration [m/s^2] | -5.0 | | max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | @@ -164,7 +167,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | | min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | -#### State transition +### State transition | Name | Type | Description | Default value | | :---------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | @@ -177,7 +180,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | | emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | -#### DRIVE Parameter +### DRIVE Parameter | Name | Type | Description | Default value | | :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | @@ -196,7 +199,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | 0.5 | | brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 | -#### STOPPING Parameter (smooth stop) +### STOPPING Parameter (smooth stop) Smooth stop is enabled if `enable_smooth_stop` is true. In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity. @@ -218,7 +221,7 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig | smooth_stop_weak_stop_dist | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m] | -0.3 | | smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m] | -0.5 | -#### STOPPED Parameter +### STOPPED Parameter | Name | Type | Description | Default value | | :----------- | :----- | :------------------------------------------- | :------------ | @@ -226,7 +229,7 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig | stopped_acc | double | target acceleration in STOPPED state [m/s^2] | -3.4 | | stopped_jerk | double | target jerk in STOPPED state [m/s^3] | -5.0 | -#### EMERGENCY Parameter +### EMERGENCY Parameter | Name | Type | Description | Default value | | :------------- | :----- | :------------------------------------------------ | :------------ | @@ -239,6 +242,3 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig ## Future extensions / Unimplemented parts ## Related issues - -- -- diff --git a/control/trajectory_follower/design/trajectory_follower-design.md b/control/trajectory_follower/design/trajectory_follower-design.md index 2ade485f1a641..46358abe91090 100644 --- a/control/trajectory_follower/design/trajectory_follower-design.md +++ b/control/trajectory_follower/design/trajectory_follower-design.md @@ -12,13 +12,49 @@ This package provides the library code used by the nodes of the `trajectory_foll Mainly, it implements two algorithms: - Model-Predictive Control (MPC) for the computation of lateral steering commands. - - [trajectory_follower-mpc-design](trajectory_follower-mpc-design.md) + - [trajectory_follower-mpc-design](mpc_lateral_controller-design.md) - PID control for the computation of velocity and acceleration commands. - - [trajectory_follower-pid-design](trajectory_follower-pid-design.md) + - [trajectory_follower-pid-design](pid_longitudinal_controller-design.md) + +## Design + +![Controller](../../trajectory_follower_nodes/design/media/Controller.drawio.svg) + +There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. +The interface class has the following base functions. + +- `setInputData()`: Input the data subscribed in [Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented with the inherited algorithm and the used data must be selected. +- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented by inherited algorithms. +- `syncData()`: Input the result of running the other controller. + - steer angle convergence + - allow keeping stopped until steer is converged. + - velocity convergence(currently not used) + +See [the Design of Trajectory Follower Nodes](../../trajectory_follower_nodes/design/trajectory_follower-design.md#Design) for how these functions work in the node. + +## Separated lateral (steering) and longitudinal (velocity) controls + +This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. + +- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. +- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. + +Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. + +### Complex requirements for longitudinal motion + +The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. + +In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. + +There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. + +### Nonlinear coupling of lateral and longitudinal motion + +The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. + +Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. ## Related issues - -- -- diff --git a/control/trajectory_follower/design/trajectory_follower-mpc-design.md b/control/trajectory_follower/design/trajectory_follower-mpc-design.md deleted file mode 100644 index 5dbdf4169651b..0000000000000 --- a/control/trajectory_follower/design/trajectory_follower-mpc-design.md +++ /dev/null @@ -1,76 +0,0 @@ -# MPC (Trajectory Follower) - -This is the design document for the MPC implemented in the `trajectory_follower` package. - -## Purpose / Use cases - - - - -Model Predictive Control (MPC) is used by the `trajectory_follower` -to calculate the lateral commands corresponding to a steering angle and a steering rate. - -This implementation differs from the one in the `mpc_controller` package in several aspects. - -- This is a linear MPC that only computes the steering command whereas - the `mpc_controller` uses a non-linear MPC which calculates coupled steering and velocity commands. -- The optimization problem solved by the linear MPC is simpler, making it less likely to fail. -- Tuning of the linear MPC is easier. - -## Design - - - - -MPC uses predictions of the vehicle's motion to optimize the immediate control command. - -Different vehicle models are implemented: - -- `kinematics` : bicycle kinematics model with steering 1st-order delay. -- `kinematics_no_delay` : bicycle kinematics model without steering delay. -- `dynamics` : bicycle dynamics model considering slip angle. - -The `kinematics` model is being used by default. Please see the reference [1] for more details. - -For the optimization, a Quadratic Programming (QP) solver is used -with two options are currently implemented: - -- `unconstraint` : use least square method to solve unconstraint QP with eigen. -- `unconstraint_fast` : similar to unconstraint. This is faster, but lower accuracy for optimization. - -## Filtering - -Filtering is required for good noise reduction. -A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for -the output steering angle. -Other filtering methods can be considered as long as the noise reduction performances are good -enough. -The moving average filter for example is not suited and can yield worse results than without any -filtering. - -## Inputs / Outputs / API - - - - -The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. -Once a vehicle model, a QP solver, and the reference trajectory to follow have been set -(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command -can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. - -## References / External links - - - -- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", - Robotics Institute, Carnegie Mellon University, February 2009. - -## Related issues - - - -- -- diff --git a/control/trajectory_follower/design/trajectory_follower-pid-design.md b/control/trajectory_follower/design/trajectory_follower-pid-design.md deleted file mode 100644 index ef94ffcc1eeda..0000000000000 --- a/control/trajectory_follower/design/trajectory_follower-pid-design.md +++ /dev/null @@ -1,95 +0,0 @@ -# PID (Trajectory Follower) - -This is the design document for the PID implemented in the `trajectory_follower` package. - -## Purpose / Use cases - - - - -PID control is used by the `trajectory_follower` -to calculate longitudinal commands corresponding to a velocity and an acceleration. - -## Design - - - - -This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity. - -This PID logic has a maximum value for the output of each term. This is to prevent the following: - -- Large integral terms may cause unintended behavior by users. -- Unintended noise may cause the output of the derivative term to be very large. - -Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as "Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start. -On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers. - -At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. -This may be replaced by a higher performance controller (adaptive control or robust control) in future development. - -@image html images/trajectory_follower-pid-diagram.svg "PID controller diagram" - -### States - -This module has four state transitions as shown below in order to handle special processing in a specific situation. - -- **DRIVE** - - Executes target velocity tracking by PID control. - - It also applies the delay compensation and slope compensation. -- **STOPPING** - - Controls the motion just before stopping. - - Special sequence is performed to achieve accurate and smooth stopping. -- **STOPPED** - - Performs operations in the stopped state (e.g. brake hold) -- **EMERGENCY**. - - Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line). - - The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters. - -The state transition diagram is shown below. - -@image html images/trajectory_follower-pid-states-diagram.svg "State Transitions" - -### Time delay compensation - -At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy. -Depending on the actuating principle of the vehicle, -the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond. - -In this controller, -the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. - -### Slope compensation - -Based on the slope information, a compensation term is added to the target acceleration. - -There are two sources of the slope information, which can be switched by a parameter. - -- Pitch of the estimated ego-pose (default) - - Calculates the current slope from the pitch angle of the estimated ego-pose - - Pros: Easily available - - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. -- Z coordinate on the trajectory - - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory - - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained - - Pros: Can be used in combination with delay compensation (not yet implemented) - - Cons: z-coordinates of high-precision map is needed. - - Cons: Does not support free space planning (for now) - -### Inputs / Outputs / API - - - - -The `PIDController` class is straightforward to use. -First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. -Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. - -## Related issues - - - -- diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index 296c7f7f6c59a..559d79cc4ace6 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -4,9 +4,18 @@ trajectory_follower 1.0.0 Library for generating lateral and longitudinal controls following a trajectory - Maxime CLEMENT + + + Takamasa Horibe + + Takayuki Murooka + Apache 2.0 + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + ament_cmake_auto autoware_cmake diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index a56dc36a007de..6fce85d88827c 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -100,13 +100,13 @@ bool8_t resampleMPCTrajectoryByDistance( std::vector input_yaw = input.yaw; convertEulerAngleToMonotonic(&input_yaw); - output->x = interpolation::slerp(input_arclength, input.x, output_arclength); - output->y = interpolation::slerp(input_arclength, input.y, output_arclength); - output->z = interpolation::slerp(input_arclength, input.z, output_arclength); - output->yaw = interpolation::slerp(input_arclength, input.yaw, output_arclength); + output->x = interpolation::spline(input_arclength, input.x, output_arclength); + output->y = interpolation::spline(input_arclength, input.y, output_arclength); + output->z = interpolation::spline(input_arclength, input.z, output_arclength); + output->yaw = interpolation::spline(input_arclength, input.yaw, output_arclength); output->vx = interpolation::lerp(input_arclength, input.vx, output_arclength); - output->k = interpolation::slerp(input_arclength, input.k, output_arclength); - output->smooth_k = interpolation::slerp(input_arclength, input.smooth_k, output_arclength); + output->k = interpolation::spline(input_arclength, input.k, output_arclength); + output->smooth_k = interpolation::spline(input_arclength, input.smooth_k, output_arclength); output->relative_time = interpolation::lerp(input_arclength, input.relative_time, output_arclength); diff --git a/control/trajectory_follower_nodes/design/latlon_muxer-design.md b/control/trajectory_follower_nodes/design/latlon_muxer-design.md deleted file mode 100644 index aa5ea71f2bfcb..0000000000000 --- a/control/trajectory_follower_nodes/design/latlon_muxer-design.md +++ /dev/null @@ -1,40 +0,0 @@ -# Lateral/Longitudinal Control Muxer - -## Purpose - -When using controllers that independently compute lateral and longitudinal commands, -this node combines the resulting messages into a single control command message. - -## Design - -Inputs. - -- `AckermannLateralCommand`: lateral command. -- `LongitudinalCommand`: longitudinal command. - -Output. - -- `AckermannControlCommand`: message containing both lateral and longitudinal commands. - -Parameter. - -- `timeout_thr_sec`: duration in second after which input messages are discarded. - -Each time the node receives an input message it publishes an `AckermannControlCommand` -if the following two conditions are met. - -1. Both inputs have been received. -2. The last received input messages are not older than defined by `timeout_thr_sec`. - -## Implementation Details - -Callbacks `latCtrlCmdCallback` and `lonCtrlCmdCallback` are defined for each input message. -Upon reception, the message is stored and function `publishCmd` is called. - -Function `publishCmd` first checks that both messages have been received -and that the stored messages are not older than the timeout. -If both conditions are true, the combined control message is built and published. - -`checkTimeout` is used to check that the stored messages are not too old -by comparing the timeout parameter `timeout_thr_sec` -with the difference between `rclcpp::Time stamp` and the current time `now()`. diff --git a/control/trajectory_follower_nodes/design/media/Controller.drawio.svg b/control/trajectory_follower_nodes/design/media/Controller.drawio.svg new file mode 100644 index 0000000000000..e5c3517233f3f --- /dev/null +++ b/control/trajectory_follower_nodes/design/media/Controller.drawio.svg @@ -0,0 +1,204 @@ + + + + + + + + + Controller + + + + + +
+
+
+
+ + LongitudinalControllerBase:  longitudinal_controller_ + +
+ +
+ + LateralControllerBase:  lateral_controller_ + +
+
+ +
+
+
+ + + LongitudinalControllerBase:  longitudinal_c... + + + + + + + + + + LongitudinalControllerBase + + + + + + + + + + PidLongitudinalController + + + + + + + + + + LateralControllerBase + + + + + + + + + + MpcLateralController + + + + + + + + + + PurePursuitLateralController + + + + + +
+
+
+ trajectory_follower_nodes/controller_node +
+
+
+ + + trajectory_follower_nodes/controller_node + + + + + + +
+
+
+ trajectory_follower/ +
+ lateral_controller_base +
+
+
+ + + trajectory_follower/... + + + + + + +
+
+
+ trajectory_follower/ +
+ longitudinal_controller_base +
+
+
+ + + trajectory_follower/... + + + + + + +
+
+
+ trajectory_follower/ +
+ mpc_lateral_controller +
+
+
+ + + trajectory_follower/... + + + + + + +
+
+
+ trajectory_follower/ +
+ pure_pursuit_lateral_controller +
+
+
+ + + trajectory_follower/... + + + + + + +
+
+
+ trajectory_follower/ +
+ pid_longitudinal_controller +
+
+
+ + + trajectory_follower/... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/control/trajectory_follower_nodes/design/trajectory_follower-design.md b/control/trajectory_follower_nodes/design/trajectory_follower-design.md index 3a1c8ab2457a5..a7b6361cff856 100644 --- a/control/trajectory_follower_nodes/design/trajectory_follower-design.md +++ b/control/trajectory_follower_nodes/design/trajectory_follower-design.md @@ -6,16 +6,49 @@ Generate control commands to follow a given Trajectory. ## Design -This functionality is decomposed into three nodes. +This is a node of the functionalities implemented in [trajectory_follower](../../trajectory_follower/design/trajectory_follower-design.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. -- [lateral-controller-design](lateral_controller-design.md) : generates lateral control messages. -- [longitudinal-controller-design](longitudinal_controller-design.md) : generates longitudinal control messages. -- [latlon-muxer-design](latlon_muxer-design.md) : combines the lateral and longitudinal control commands - into a single control command. +![Controller](media/Controller.drawio.svg) -Core functionalities are implemented in the [trajectory_follower](../../trajectory_follower/design/trajectory_follower-design.md#trajectory-follower) package. +The process flow is as follows. -@image html images/trajectory_follower-diagram.png "Overview of the Trajectory Follower package" +1. Set input data to the lateral controller +2. Compute lateral commands. +3. Sync the result of the lateral control to the longitudinal controller. + - steer angle convergence +4. Set input data to the longitudinal controller +5. Compute longitudinal commands. +6. Sync the result of the longitudinal control to the longitudinal controller. + - velocity convergence(currently not used) + +Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` + +- lateral controller + - `keep_steer_control_until_converged` +- longitudinal controller + - `enable_keep_stopped_until_steer_convergence` + +### Inputs / Outputs / API + +#### Inputs + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_auto_vehicle_msgs/SteeringReport` current steering + +#### Outputs + +- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. + +#### Parameter + +- `ctrl_period`: control commands publishing period +- `timeout_thr_sec`: duration in second after which input messages are discarded. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. + 1. Both commands have been received. + 2. The last received commands are not older than defined by `timeout_thr_sec`. +- `lateral_controller_mode`: `mpc_follower` or `pure_pursuit` + - (currently there is only `PID` for longitudinal controller) ## Debugging diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_nodes/package.xml index b3a982b0a6b9b..de184228e5f87 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_nodes/package.xml @@ -4,9 +4,18 @@ trajectory_follower_nodes 1.0.0 Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands - Maxime CLEMENT + + + Takamasa Horibe + + Takayuki Murooka + Apache License 2.0 + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + ament_cmake_auto autoware_cmake diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index f21ef8f81fa6c..a6d98fa682d74 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml @@ -56,7 +56,7 @@ stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 converged_steer_rad: 0.1 - keep_steer_control_until_converged: false + keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml index e5e82ec101be2..eb2ef443c4576 100644 --- a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml @@ -6,7 +6,7 @@ enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: false - enable_keep_stopped_until_steer_convergence: false + enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 diff --git a/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp b/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp deleted file mode 100644 index bb1fd9d3b0bc6..0000000000000 --- a/control/trajectory_follower_nodes/test/test_latlon_muxer_node.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "fake_test_node/fake_test_node.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "trajectory_follower_test_utils.hpp" - -#include - -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" - -#include -#include -#include - -using LatLonMuxer = autoware::motion::control::trajectory_follower_nodes::LatLonMuxer; -using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; -using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; -using ControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; - -using FakeNodeFixture = autoware::tools::testing::FakeTestNode; - -const rclcpp::Duration one_second(1, 0); - -TEST_F(FakeNodeFixture, TestCorrectOutput) -{ - // Data to test - ControlCommand::SharedPtr cmd_msg; - bool received_combined_command = false; - // Node - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("timeout_thr_sec", 0.5); - std::shared_ptr node = std::make_shared(node_options); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr lat_pub = - this->create_publisher("latlon_muxer/input/lateral/control_cmd"); - rclcpp::Publisher::SharedPtr lon_pub = - this->create_publisher("latlon_muxer/input/longitudinal/control_cmd"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "latlon_muxer/output/control_cmd", *node, - [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_combined_command = true; - }); - // Publish messages - LateralCommand lat_msg; - lat_msg.steering_tire_angle = 1.5; - lat_msg.steering_tire_rotation_rate = 0.2f; - LongitudinalCommand lon_msg; - lon_msg.speed = 5.0; - lon_msg.acceleration = -1.0; - lon_msg.jerk = 0.25; - lat_msg.stamp = node->now(); - lon_msg.stamp = node->now(); - lat_pub->publish(lat_msg); - lon_pub->publish(lon_msg); - - test_utils::waitForMessage(node, this, received_combined_command); - // Ensure the combined control command was published and contains correct data - ASSERT_TRUE(received_combined_command); - EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, lat_msg.steering_tire_angle); - EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, lat_msg.steering_tire_rotation_rate); - EXPECT_EQ(cmd_msg->longitudinal.speed, lon_msg.speed); - EXPECT_EQ(cmd_msg->longitudinal.acceleration, lon_msg.acceleration); - EXPECT_EQ(cmd_msg->longitudinal.jerk, lon_msg.jerk); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(lat_msg.stamp)); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(lon_msg.stamp)); -} - -TEST_F(FakeNodeFixture, TestLateralTimeout) -{ - // Data to test - ControlCommand::SharedPtr cmd_msg; - bool received_combined_command = false; - // Node - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("timeout_thr_sec", 0.5); - std::shared_ptr node = std::make_shared(node_options); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr lat_pub = - this->create_publisher("latlon_muxer/input/lateral/control_cmd"); - rclcpp::Publisher::SharedPtr lon_pub = - this->create_publisher("latlon_muxer/input/longitudinal/control_cmd"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "latlon_muxer/output/control_cmd", *node, - [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_combined_command = true; - }); - // Publish messages - LateralCommand lat_msg; - LongitudinalCommand lon_msg; - // Generate a timeout of the lateral message - lat_msg.stamp = node->now() - one_second; - lon_msg.stamp = node->now(); - lat_pub->publish(lat_msg); - lon_pub->publish(lon_msg); - - test_utils::waitForMessage( - node, this, received_combined_command, std::chrono::seconds{1LL}, false); - // Ensure combined command was not published - ASSERT_FALSE(received_combined_command); -} - -TEST_F(FakeNodeFixture, TestLongitudinalTimeout) -{ - // Data to test - ControlCommand::SharedPtr cmd_msg; - bool received_combined_command = false; - // Node - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("timeout_thr_sec", 0.5); - std::shared_ptr node = std::make_shared(node_options); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr lat_pub = - this->create_publisher("latlon_muxer/input/lateral/control_cmd"); - rclcpp::Publisher::SharedPtr lon_pub = - this->create_publisher("latlon_muxer/input/longitudinal/control_cmd"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "latlon_muxer/output/control_cmd", *node, - [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_combined_command = true; - }); - // Publish messages - LateralCommand lat_msg; - LongitudinalCommand lon_msg; - // Generate a timeout of the longitudinal message - lat_msg.stamp = node->now(); - lon_msg.stamp = node->now() - one_second; - lat_pub->publish(lat_msg); - lon_pub->publish(lon_msg); - - test_utils::waitForMessage( - node, this, received_combined_command, std::chrono::seconds{1LL}, false); - // Ensure combined command was not published - ASSERT_FALSE(received_combined_command); -} - -TEST_F(FakeNodeFixture, TestLatlonTimeout) -{ - // Data to test - ControlCommand::SharedPtr cmd_msg; - bool received_combined_command = false; - // Node - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("timeout_thr_sec", 0.5); - std::shared_ptr node = std::make_shared(node_options); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr lat_pub = - this->create_publisher("latlon_muxer/input/lateral/control_cmd"); - rclcpp::Publisher::SharedPtr lon_pub = - this->create_publisher("latlon_muxer/input/longitudinal/control_cmd"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "latlon_muxer/output/control_cmd", *node, - [&cmd_msg, &received_combined_command](const ControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_combined_command = true; - }); - // Publish messages - LateralCommand lat_msg; - LongitudinalCommand lon_msg; - // Generate a timeout of the longitudinal message - lat_msg.stamp = node->now() - one_second; - lon_msg.stamp = node->now() - one_second; - lat_pub->publish(lat_msg); - lon_pub->publish(lon_msg); - - test_utils::waitForMessage( - node, this, received_combined_command, std::chrono::seconds{1LL}, false); - // Ensure combined command was not published - ASSERT_FALSE(received_combined_command); -} diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index f2bb7733a3c61..02b798ed49bd4 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -4,9 +4,11 @@ vehicle_cmd_gate 0.1.0 The vehicle_cmd_gate package - h_ohta + Takamasa Horibe Apache License 2.0 + Hiroki OTA + ament_cmake autoware_cmake diff --git a/launch/tier4_autoware_api_launch/README.md b/launch/tier4_autoware_api_launch/README.md index e2ea11b7de4c1..fe409160f8e51 100644 --- a/launch/tier4_autoware_api_launch/README.md +++ b/launch/tier4_autoware_api_launch/README.md @@ -10,13 +10,10 @@ Please see `` in `package.xml`. ## Usage -You can include as follows in `*.launch.xml` to use `control.launch.py`. +You can include as follows in `*.launch.xml` to use `autoware_api.launch.xml`. ```xml - - - - + ``` ## Notes diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py index 4b8513f544588..422c35abee354 100644 --- a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py @@ -13,7 +13,6 @@ # limitations under the License. import launch -from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode @@ -29,12 +28,7 @@ def _create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): - param_initial_pose = { - "init_simulator_pose": LaunchConfiguration("init_simulator_pose"), - "init_localization_pose": LaunchConfiguration("init_localization_pose"), - } components = [ - _create_api_node("initial_pose", "InitialPose", parameters=[param_initial_pose]), _create_api_node("iv_msgs", "IVMsgs"), _create_api_node("operator", "Operator"), _create_api_node("route", "Route"), diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml index 4723621f84d68..27c62523ab6bd 100644 --- a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml @@ -16,8 +16,5 @@ name="crosswalk_states" args="/api/autoware/set/crosswalk_states /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/input/external_crosswalk_states" /> - - - diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 2a42e2bb52f55..9b7abb00a8c91 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -5,7 +5,7 @@ 0.0.0 The tier4_autoware_api_launch package Takagi, Isamu - Ryohsuke, Mitsudome + Ryohsuke Mitsudome Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index e69e599d198ec..c786daee07d24 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -57,7 +57,7 @@ stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 converged_steer_rad: 0.1 - keep_steer_control_until_converged: false + keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 diff --git a/launch/tier4_control_launch/config/trajectory_follower/latlon_muxer.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/latlon_muxer.param.yaml deleted file mode 100644 index 371bb99787559..0000000000000 --- a/launch/tier4_control_launch/config/trajectory_follower/latlon_muxer.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - timeout_thr_sec: 0.5 # [s] Time limit after which input messages are discarded. diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index 28fa297afbecb..b6e1c3a38c799 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -6,7 +6,7 @@ enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: false - enable_keep_stopped_until_steer_convergence: false + enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 2555c9175d697..8d9b3d90d9fa1 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -4,7 +4,7 @@ tier4_control_launch 0.1.0 The tier4_control_launch package - Takamasa Horibe + Takamasa Horibe Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index bf083a97bb1bf..71525748d5f2d 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -13,6 +13,7 @@ + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 5ee9d81cb13f9..683d7b7ed533f 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -4,7 +4,7 @@ tier4_map_launch 0.1.0 The tier4_map_launch package - mitsudome-r + Ryohsuke Mitsudome Apache License 2.0 ament_cmake_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 1a24caffd6dbe..7cfcb2fc81e1b 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 @@ -23,6 +23,7 @@ + @@ -214,16 +215,18 @@ + + - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index d7f8eae562074..c11ca9fbc48d9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -4,6 +4,7 @@ + @@ -50,6 +51,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index b025068d0e220..cf42be9cee1a1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -7,6 +7,7 @@ + @@ -54,6 +55,7 @@ + @@ -83,6 +85,7 @@ + @@ -94,6 +97,7 @@ + @@ -106,6 +110,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 1169eba8dcda7..318a5fdffab9f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -6,6 +6,7 @@ + @@ -118,16 +119,18 @@ + + - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index ca49d9139e8f0..168d837755417 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -4,6 +4,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 90066be1144a1..ae6df48a412f2 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -27,6 +27,7 @@ + + diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml index 434545674d283..7a9a63f805d67 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -1,25 +1,30 @@ /**: ros__parameters: pull_out: - min_stop_distance: 2.0 - stop_time: 0.0 - hysteresis_buffer_distance: 1.0 - pull_out_prepare_duration: 4.0 - pull_out_duration: 2.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 pull_out_finish_judge_buffer: 1.0 - minimum_pull_out_velocity: 2.0 - prediction_duration: 30.0 - prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - use_dynamic_object: true - enable_blocked_by_obstacle: false - pull_out_search_distance: 30.0 - before_pull_out_straight_distance: 5.0 - after_pull_out_straight_distance: 5.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + before_pull_out_straight_distance: 0.0 + minimum_shift_pull_out_distance: 20.0 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 + # geometric pull out + enable_geometric_pull_out: true + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 15.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 9653a75318b2a..d11ed1f0db74f 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -2,20 +2,17 @@ ros__parameters: pull_over: request_length: 100.0 - th_arrived_distance_m: 1.0 - th_stopped_velocity_mps: 0.01 - th_stopped_time_sec: 2.0 # It must be greater than the state_machine's. + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. pull_over_velocity: 3.0 pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 decide_path_distance: 10.0 - min_acc: -0.5 - enable_shift_parking: true - enable_arc_forward_parking: true - enable_arc_backward_parking: true + maximum_deceleration: 0.5 # goal research - search_priority: "efficient_path" # "efficient_path" or "close_goal" enable_goal_research: true + search_priority: "efficient_path" # "efficient_path" or "close_goal" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 goal_search_interval: 1.0 @@ -25,20 +22,25 @@ theta_size: 360 obstacle_threshold: 60 # shift path + enable_shift_parking: true pull_over_sampling_num: 4 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - maximum_deceleration: 1.0 after_pull_over_straight_distance: 5.0 before_pull_over_straight_distance: 5.0 # parallel parking path + enable_arc_forward_parking: true + enable_arc_backward_parking: true after_forward_parking_straight_distance: 2.0 after_backward_parking_straight_distance: 2.0 forward_parking_velocity: 0.3 backward_parking_velocity: -0.3 + forward_parking_lane_departure_margin: 0.0 + backward_parking_lane_departure_margin: 0.0 arc_path_interval: 1.0 - # hazard_on when parked + pull_over_max_steer_angle: 0.35 # 20deg + # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 # check safety with dynamic objects. Not used now. diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index eb07deaa2ce23..0ea817ffefc81 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr + keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 50cadf6101f0f..e7b0bdbdd3bfe 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -4,7 +4,7 @@ tier4_planning_launch 0.1.0 The tier4_planning_launch package - Takamasa Horibe + Takamasa Horibe Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml index f8dc832d7b2f2..e91aa4ca3fbb3 100644 --- a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml +++ b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml @@ -3,3 +3,5 @@ devices: ["*"] traffic_reader_port: 7636 monitor_program: "greengrass" + crc_error_check_duration: 1 + crc_error_count_threshold: 1 diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 79d0abc7b7c9b..0b1fc2517cbee 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -11,11 +11,39 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_executable(ekf_localizer - src/ekf_localizer_node.cpp +ament_auto_find_build_dependencies() + +ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_localizer.cpp + src/state_transition.cpp ) -ament_target_dependencies(ekf_localizer kalman_filter) + +target_link_libraries(ekf_localizer_lib Eigen3::Eigen) + +ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp) +target_link_libraries(ekf_localizer ekf_localizer_lib) +target_include_directories(ekf_localizer PUBLIC include) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + + ament_add_gtest(${test_name} ${filepath}) + target_link_libraries("${test_name}" ekf_localizer_lib) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + set(TEST_FILES + test/test_state_transition.cpp) + + foreach(filepath ${TEST_FILES}) + add_testcase(${filepath}) + endforeach() +endif() + # if(BUILD_TESTING) # find_package(ament_cmake_ros REQUIRED) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index c9a7a0dc6014c..0c3fa0ae8c3f1 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -198,15 +198,6 @@ class EKFLocalizer : public rclcpp::Node bool is_initialized_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - YAWB = 3, - VX = 4, - WZ = 5, - }; - /* for model prediction */ std::queue current_twist_info_queue_; //!< @brief current measured pose std::queue current_pose_info_queue_; //!< @brief current measured pose @@ -291,13 +282,6 @@ class EKFLocalizer : public rclcpp::Node std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform); - /** - * @brief normalize yaw angle - * @param yaw yaw angle - * @return normalized yaw - */ - double normalizeYaw(const double & yaw) const; - /** * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_ */ diff --git a/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp b/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp new file mode 100644 index 0000000000000..732abd6c8e2e7 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp @@ -0,0 +1,23 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__MATRIX_TYPES_HPP_ +#define EKF_LOCALIZER__MATRIX_TYPES_HPP_ + +#include + +using Vector6d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; + +#endif // EKF_LOCALIZER__MATRIX_TYPES_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/state_index.hpp b/localization/ekf_localizer/include/ekf_localizer/state_index.hpp new file mode 100644 index 0000000000000..b2bfdb0bd9e2e --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/state_index.hpp @@ -0,0 +1,27 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__STATE_INDEX_HPP_ +#define EKF_LOCALIZER__STATE_INDEX_HPP_ + +enum IDX { + X = 0, + Y = 1, + YAW = 2, + YAWB = 3, + VX = 4, + WZ = 5, +}; + +#endif // EKF_LOCALIZER__STATE_INDEX_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp new file mode 100644 index 0000000000000..85a65828e4ee4 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp @@ -0,0 +1,26 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__STATE_TRANSITION_HPP_ +#define EKF_LOCALIZER__STATE_TRANSITION_HPP_ + +#include "ekf_localizer/matrix_types.hpp" + +double normalizeYaw(const double & yaw); +Vector6d predictNextState(const Vector6d & X_curr, const double dt); +Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt); +Matrix6d processNoiseCovariance( + const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d); + +#endif // EKF_LOCALIZER__STATE_TRANSITION_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 41ae99015f871..00f6b760efaef 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -14,6 +14,10 @@ #include "ekf_localizer/ekf_localizer.hpp" +#include "ekf_localizer/matrix_types.hpp" +#include "ekf_localizer/state_index.hpp" +#include "ekf_localizer/state_transition.hpp" + #include #include @@ -406,47 +410,17 @@ void EKFLocalizer::predictKinematicsModel() */ Eigen::MatrixXd X_curr(dim_x_, 1); // current state - Eigen::MatrixXd X_next(dim_x_, 1); // predicted state ekf_.getLatestX(X_curr); DEBUG_PRINT_MAT(X_curr.transpose()); Eigen::MatrixXd P_curr; ekf_.getLatestP(P_curr); - const double yaw = X_curr(IDX::YAW); - const double yaw_bias = X_curr(IDX::YAWB); - const double vx = X_curr(IDX::VX); - const double wz = X_curr(IDX::WZ); const double dt = ekf_dt_; - /* Update for latest state */ - X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz)*dt; // dyaw = omega + omega_bias - X_next(IDX::YAWB) = yaw_bias; - X_next(IDX::VX) = vx; - X_next(IDX::WZ) = wz; - - X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW))); - - /* Set A matrix for latest state */ - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_); - A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - A(IDX::YAW, IDX::WZ) = dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - - Q(IDX::X, IDX::X) = 0.0; - Q(IDX::Y, IDX::Y) = 0.0; - Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_; // for yaw - Q(IDX::YAWB, IDX::YAWB) = 0.0; - Q(IDX::VX, IDX::VX) = proc_cov_vx_d_; // for vx - Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_; // for wz + const Vector6d X_next = predictNextState(X_curr, dt); + const Matrix6d A = createStateTransitionMatrix(X_curr, dt); + const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d_, proc_cov_vx_d_, proc_cov_wz_d_); ekf_.predictWithDelay(X_next, A, Q); @@ -760,11 +734,6 @@ void EKFLocalizer::publishEstimateResult() pub_debug_->publish(msg); } -double EKFLocalizer::normalizeYaw(const double & yaw) const -{ - return std::atan2(std::sin(yaw), std::cos(yaw)); -} - void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { double z = pose.pose.pose.position.z; diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp new file mode 100644 index 0000000000000..0f8bb091c3662 --- /dev/null +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -0,0 +1,75 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/matrix_types.hpp" +#include "ekf_localizer/state_index.hpp" + +#include + +double normalizeYaw(const double & yaw) +{ + // FIXME(IshitaTakeshi) I think the computation here can be simplified + // FIXME(IshitaTakeshi) Rename the function. This is not normalization + return std::atan2(std::sin(yaw), std::cos(yaw)); +} + +Vector6d predictNextState(const Vector6d & X_curr, const double dt) +{ + const double x = X_curr(IDX::X); + const double y = X_curr(IDX::Y); + const double yaw = X_curr(IDX::YAW); + const double yaw_bias = X_curr(IDX::YAWB); + const double vx = X_curr(IDX::VX); + const double wz = X_curr(IDX::WZ); + + Vector6d X_next; + X_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + X_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + X_next(IDX::YAW) = normalizeYaw(yaw + wz * dt); // dyaw = omega + omega_bias + X_next(IDX::YAWB) = yaw_bias; + X_next(IDX::VX) = vx; + X_next(IDX::WZ) = wz; + return X_next; +} + +// TODO(TakeshiIshita) show where the equation come from +Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt) +{ + const double yaw = X_curr(IDX::YAW); + const double yaw_bias = X_curr(IDX::YAWB); + const double vx = X_curr(IDX::VX); + + Matrix6d A = Matrix6d::Identity(); + A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; + A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; + A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; + A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; + A(IDX::YAW, IDX::WZ) = dt; + return A; +} + +Matrix6d processNoiseCovariance( + const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d) +{ + Matrix6d Q = Matrix6d::Zero(); + Q(IDX::X, IDX::X) = 0.0; + Q(IDX::Y, IDX::Y) = 0.0; + Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw + Q(IDX::YAWB, IDX::YAWB) = 0.0; + Q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx + Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz + return Q; +} diff --git a/localization/ekf_localizer/test/test_state_transition.cpp b/localization/ekf_localizer/test/test_state_transition.cpp new file mode 100644 index 0000000000000..9cb7975a964a9 --- /dev/null +++ b/localization/ekf_localizer/test/test_state_transition.cpp @@ -0,0 +1,95 @@ +// Copyright 2018-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. + +#define _USE_MATH_DEFINES +#include "ekf_localizer/state_index.hpp" +#include "ekf_localizer/state_transition.hpp" + +#include +#include + +TEST(StateTransition, NormalizeYaw) +{ + const double tolerance = 1e-6; + EXPECT_NEAR(normalizeYaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalizeYaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalizeYaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); + EXPECT_NEAR(normalizeYaw(M_PI * 4), M_PI * 0, tolerance); +} + +TEST(PredictNextState, PredictNextState) +{ + // This function is the definition of state transition so we just check + // if the calculation is done according to the formula + Vector6d X_curr; + X_curr(0) = 2.; + X_curr(1) = 3.; + X_curr(2) = M_PI / 2.; + X_curr(3) = M_PI / 4.; + X_curr(4) = 10.; + X_curr(5) = 2. * M_PI / 3.; + + const double dt = 0.5; + + const Vector6d X_next = predictNextState(X_curr, dt); + + const double tolerance = 1e-10; + EXPECT_NEAR(X_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(X_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(X_next(2), normalizeYaw(M_PI / 2. + M_PI / 3.), tolerance); + EXPECT_NEAR(X_next(3), X_curr(3), tolerance); + EXPECT_NEAR(X_next(4), X_curr(4), tolerance); + EXPECT_NEAR(X_next(5), X_curr(5), tolerance); +} + +TEST(CreateStateTransitionMatrix, NumericalApproximation) +{ + // The transition matrix A = df / dx + // We check if df = A * dx approximates f(x + dx) - f(x) + + { + // check around x = 0 + const double dt = 0.1; + const Vector6d dx = 0.1 * Vector6d::Ones(); + const Vector6d x = Vector6d::Zero(); + + const Matrix6d A = createStateTransitionMatrix(x, dt); + const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + + EXPECT_LT((df - A * dx).norm(), 2e-3); + } + + { + // check around random x + const double dt = 0.1; + const Vector6d dx = 0.1 * Vector6d::Ones(); + const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished(); + + const Matrix6d A = createStateTransitionMatrix(x, dt); + const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + + EXPECT_LT((df - A * dx).norm(), 5e-3); + } +} + +TEST(ProcessNoiseCovariance, ProcessNoiseCovariance) +{ + const Matrix6d Q = processNoiseCovariance(1., 2., 3.); + EXPECT_EQ(Q(2, 2), 1.); // for yaw + EXPECT_EQ(Q(4, 4), 2.); // for vx + EXPECT_EQ(Q(5, 5), 3.); // for wz + + // Make sure other elements are zero + EXPECT_EQ(processNoiseCovariance(0, 0, 0).norm(), 0.); +} diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index a2afc96c24e50..1f1f258b1fec5 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -4,7 +4,7 @@ map_loader 0.1.0 The map_loader package - mitsudome-r + Ryohsuke Mitsudome Kenji Miyake Apache License 2.0 diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index d416d6a455593..216dffe817449 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -4,7 +4,7 @@ lanelet2_map_preprocessor 0.1.0 The lanelet2_map_preprocessor package - mitsudome-r + Ryohsuke Mitsudome Apache License 2.0 ament_cmake_auto diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/lidar_apollo_instance_segmentation/package.xml index f1a001ce3d563..21cdf36c401e1 100755 --- a/perception/lidar_apollo_instance_segmentation/package.xml +++ b/perception/lidar_apollo_instance_segmentation/package.xml @@ -7,7 +7,7 @@ Yukihiro Saito Apache License 2.0 - Kosuke Takeuchi + Kosuke Takeuchi Yukihiro Saito ament_cmake_auto diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 326964eefdeae..fb2be1dd610ba 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -6,6 +6,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 57237609139e5..8bc5f955efab4 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -71,13 +71,15 @@ void box3DToDetectedObject( tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); // twist - float vel_x = box3d.vel_x; - float vel_y = box3d.vel_y; - geometry_msgs::msg::Twist twist; - twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); - twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); - obj.kinematics.twist_with_covariance.twist = twist; - obj.kinematics.has_twist = has_twist; + if (has_twist) { + float vel_x = box3d.vel_x; + float vel_y = box3d.vel_y; + geometry_msgs::msg::Twist twist; + twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); + twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); + obj.kinematics.twist_with_covariance.twist = twist; + obj.kinematics.has_twist = has_twist; + } } uint8_t getSemanticType(const std::string & class_name) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 4d594f46c8af8..093bc5c76f3b8 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -309,28 +309,28 @@ PosePath PathGenerator::interpolateReferencePath( } // Spline Interpolation - std::vector slerp_ref_path_x = - interpolation::slerp(base_path_s, base_path_x, resampled_s); - std::vector slerp_ref_path_y = - interpolation::slerp(base_path_s, base_path_y, resampled_s); - std::vector slerp_ref_path_z = - interpolation::slerp(base_path_s, base_path_z, resampled_s); + std::vector spline_ref_path_x = + interpolation::spline(base_path_s, base_path_x, resampled_s); + std::vector spline_ref_path_y = + interpolation::spline(base_path_s, base_path_y, resampled_s); + std::vector spline_ref_path_z = + interpolation::spline(base_path_s, base_path_z, resampled_s); interpolated_path.resize(interpolate_num); for (size_t i = 0; i < interpolate_num - 1; ++i) { geometry_msgs::msg::Pose interpolated_pose; const auto current_point = - tier4_autoware_utils::createPoint(slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), 0.0); + tier4_autoware_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); const auto next_point = tier4_autoware_utils::createPoint( - slerp_ref_path_x.at(i + 1), slerp_ref_path_y.at(i + 1), 0.0); + spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point); interpolated_pose.position = tier4_autoware_utils::createPoint( - slerp_ref_path_x.at(i), slerp_ref_path_y.at(i), slerp_ref_path_z.at(i)); + spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); interpolated_path.at(i) = interpolated_pose; } interpolated_path.back().position = tier4_autoware_utils::createPoint( - slerp_ref_path_x.back(), slerp_ref_path_y.back(), slerp_ref_path_z.back()); + spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; return interpolated_path; diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml index 93119d77e354d..bee64d933d9d8 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/radar_tracks_msgs_converter/package.xml @@ -6,6 +6,7 @@ radar_tracks_msgs_converter Satoshi Tanaka Apache License 2.0 + Satoshi Tanaka ament_cmake_auto diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 757a3b7ecad34..5036e6ae1649b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -22,10 +22,13 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/lane_following/lane_following_module.cpp src/scene_module/lane_change/lane_change_module.cpp src/scene_module/lane_change/util.cpp + src/scene_module/lane_change/debug.cpp src/scene_module/pull_over/pull_over_module.cpp src/scene_module/pull_over/util.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/pull_out/util.cpp + src/scene_module/pull_out/shift_pull_out.cpp + src/scene_module/pull_out/geometric_pull_out.cpp src/scene_module/utils/geometric_parallel_parking.cpp src/scene_module/utils/occupancy_grid_based_collision_detector.cpp src/scene_module/utils/path_shifter.cpp diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index f07711647837a..a1e4c31c49ce7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -15,7 +15,7 @@ The following modules are currently supported: - **Lane Following**: Generate lane centerline from map. - **Lane Change**: Performs a lane change. This module is performed when it is necessary and a collision check with other vehicles is cleared. - **Obstacle Avoidance**: Perform an obstacle avoidance. This module is for avoidance of a vehicle parked on the edge of the lane or overtaking a low-speed obstacle. -- **Pull Over**: Performs a pull over. This module is performed when goal is in the shoulder lane. Ego-vehicle will stop at the goal. +- **Pull Over**: Performs a pull over. This module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. - **Pull Out**: Performs a pull out. This module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. - **Side Shift**: (For remote control) Shift the path to left or right according to an external instruction. @@ -46,6 +46,7 @@ The following modules are currently supported: - /planning/mission_planning/route [`autoware_auto_planning_msgs/HADMapRoute`] : Current route from start to goal. - /map/vector_map [autoware_auto_mapping_msgs/HADMapBin] : Map information. - /perception/object_recognition/objects [`autoware_auto_perception_msgs/PredictedObjects`] : dynamic objects from perception module. +- /perception/occupancy_grid_map/map [nav_msgs/msg/OccupancyGrid] : occupancy grid map from perception module. This is used for only Pull Over module - /tf [`tf2_msgs/TFMessage`] : For ego-pose. - /localization/kinematic_state [`nav_msgs/Odometry] : For ego-velocity. - path_change_approval [`std_msgs::Bool`] : (For remote control) @@ -195,32 +196,79 @@ The Pull Over module is activated when goal is in the shoulder lane. Ego-vehicle - Pull over request condition - - The goal is in shoulder lane - - The distance from ego-vehicle to the destination is long enough for pull over - - The distance required for a pull over is determined by the sum of the straight distance before pull over, the straight distance after pull over, and the minimum distance required for pull over + - The goal is in shoulder lane and the ego-vehicle is in road lane. + - The distance between the goal and ego-vehicle is somewhat close. + - It is shorter than `request_length`(default: < `100m`). - Pull over ready condition - - The end of generated path is located before the end of the road - - Distance required for pull over of generated path is shorter than distance from ego-pose to goal-pose - - Generated Path of the pull over doesn’t collide with other objects + - It is always ready if the conditions of the request are met. + +- Pull over start condition + - Generate safe parking goal and path. + - The generated path does not collide with obstacles. - Pull over is allowed by an operator - If pull over path is not allowed by an operator, leave distance required for pull over and stop. - This case is defined as UC ID: UC-F-11-00004 at [ODD Use Case Slide Pullover draft](https://docs.google.com/presentation/d/19-G1vj0rG-1P1RKWLg-Iq3_7YgcIYgXZ1wq2_J9NXGo/edit#slide=id.gf4f59f078d_0_1) #### **finish pull over condition** (need to meet any of the conditions below) - The distance to the goal from your vehicle is lower than threshold (default: < `1m`) -- The speed of the vehicle is 0. +- The ego-vehicle is stopped. + - The speed is lower than threshold (default: < `0.01m/s`). -#### **Collision prediction with obstacles** +#### General parameters for pull_over -1. Predict each position of the ego-vehicle and other vehicle on the target lane of the pull over at t1, t2,...tn -2. If a distance between the ego-vehicle and other one is lower than the threshold (`ego_velocity * stop_time (2s)`) at each time, that is judged as a collision +| Name | Unit | Type | Description | Default value | +| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| request_length | [m] | double | when the ego-vehicle approaches the goal by this distance, the module is activated. | 100.0 | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 2.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 0.3 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 0.5 | + +#### **collision check** + +##### **occupancy grid based collision check** + +Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. + +##### Parameters for occupancy grid based collision check + +| Name | Unit | Type | Description | Default value | +| :--------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.5 | +| theta_size | - | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | - | int | threshold of cell values to be considered as obstacles | 60 | + +#### **Goal Search** + +If it is not possible to park safely at a given goal, `/planning/scenario_planning/modified_goal` is +searched for in certain range of the shoulder lane. + +[Video of how goal search works](https://user-images.githubusercontent.com/39142679/178033628-bec1bbc7-3b27-47b1-b50b-55f855e2e399.mp4) + +##### Parameters for goal search + +| Name | Unit | Type | Description | Default value | +| :-------------------------- | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | +| search_priority | - | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| enable_goal_research | - | double | flag whether to search goal | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 1.0 | +| goal_to_obj_margin | [m] | double | margin between ego-vehicle and obstacles when arriving the goal | 2.0 | #### **Path Generation** +There are three path generation methods. The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. + +##### **shift parking** + Pull over distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one satisfies ready conditions described above is output. @@ -228,21 +276,75 @@ The lateral jerk is searched for among the predetermined minimum and maximum val 2. In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) 3. Combine this path with center line of road lane -![pull_over](./image/pull_over_fig1.drawio.svg) +![shift_parking](./image/shift_parking.drawio.svg) -#### Parameters for path generation +[Video of how shift_parking works](https://user-images.githubusercontent.com/39142679/178034101-4dc61a33-bc49-41a0-a9a8-755cce53cbc6.mp4) + +###### Parameters for shift parking + +| Name | Unit | Type | Description | Default value | +| :--------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | +| before_pull_over_straight_distance | [m] | double | straight line distance before pull over end point. a safe path should have a distance that includes this | 5.0 | + +##### **geometric parallel parking** + +Generate two arc paths with discontinuous curvature. It stops twice in the middle of the path to control the steer on the spot. There are two path generation methods: forward and backward. +See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. There is also [a simple python implementation](https://github.com/kosuke55/geometric-parallel-parking). + +###### Parameters geometric parallel parking + +| Name | Unit | Type | Description | Default value | +| :---------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| arc_path_interval | [m] | double | interval between arc path points | 1.0 | +| pull_over_max_steer_rad | [rad] | double | maximum steer angle for path generation. it may not be possible to control steer up to max_steer_angle in vehicle_info when stopped | 0.35 | + +###### arc forward parking + +Generate two forward arc paths. + +![arc_forward_parking](./image/arc_forward_parking.drawio.svg) + +[Video of how arc_forward_parking works](https://user-images.githubusercontent.com/39142679/178034128-4754c401-8aff-4745-b69a-4a69ca29ce4b.mp4) + +###### Parameters arc forward parking + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------- | :------------ | +| enable_arc_forward_parking | [-] | bool | flag whether to enable arc forward parking | true | +| after_forward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | +| forward_parking_velocity | [m/s] | double | velocity when forward parking | 0.3 | +| forward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front left corner of ego-vehicle when forkward parking | 0.0 | -| Name | Unit | Type | Description | Default value | -| :------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | -| straight_distance | [m] | double | straight distance after pull over. | 5.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk to calculate shifting distance. | 0.5 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk to calculate shifting distance. | 2.0 | -| pull_over_velocity | [m/s] | double | Accelerate/decelerate to this speed before the start of the pullover. | 3.0 | -| margin | [m] | double | distance from ego-vehicle's footprint to the edge of the shoulder lane. This value determines shift length. | 0.5 | +###### arc backward parking + +Generate two backward arc paths. + +![arc_backward_parking](./image/arc_backward_parking.drawio.svg). + +[Video of how arc_forward_parking works](https://user-images.githubusercontent.com/39142679/178034280-4b6754fe-3981-4aee-b5e0-970f34563c6d.mp4) + +###### Parameters arc backward parking + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------- | :---- | :----- | :------------------------------------------------------------------------ | :------------ | +| enable_arc_backward_parking | [-] | bool | flag whether to enable arc backward parking | true | +| after_backward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | +| backward_parking_velocity | [m/s] | double | velocity when backward parking | -0.3 | +| backward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front right corner of ego-vehicle when backward | 0.0 | #### Unimplemented parts / limitations for pull over -- When parking on the shoulder of the road, 3.5m space on the right side should be secured +- parking on the right shoulder is not allowed +- if the distance from the edge of the shoulder is too narrow, parking is not possible. + - if `margin_from_boundary` is set to `0.0`, etc., a path without deviation cannot be found. +- geometric_parallel_parking assumes road lanes and shoulder lanes are straight and parallel. + - sift parking is possible on curved lanes. ### Pull Out @@ -253,39 +355,109 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of - Pull out request condition - The speed of the vehicle is 0. - - The footprint of ego-vehicle is included in shoulder lane - - The distance from ego-vehicle to the destination is long enough for pull out (same with pull over) - - The distance required for a pull out is determined by the sum of the straight distance before pull out, the straight distance after pull out, and the minimum distance required for pull out + - Somewhere in footprint of ego-vehicle is included in shoulder lane + - The distance from ego-vehicle to the destination is long enough for pull out - Pull out ready condition - - The end of generated path is located before the end of the road - - Distance required for pull out of generated path is shorter than distance from ego-pose to goal-pose - - Generated Path is safe (This condition is temporarily invalid) - - It is possible to enable or disable this condition. (default: `disabled`) + - It is always ready if the conditions of the request are met. + +- Pull out start condition + - Generate safe path which does not collide with obstacles - If safe path cannot be generated from the current position, safe path from a receding position is searched. - Pull out is allowed by an operator #### **finish pull out condition** (need to meet any of the conditions below) -- The distance to the goal of your vehicle is lower than threshold (default: < `1m`) -- The speed of the vehicle is 0. +- Exceeding the pull out end point by more than the threshold (default: `1.0m`) + +#### General parameters for pull_out + +| Name | Unit | Type | Description | Default value | +| :--------------------------- | :---- | :----- | :------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| pull_out_finish_judge_buffer | [m] | double | threshold for finish judgment distance from pull out end point | 1.0 | #### **Safe check with obstacles in shoulder lane** -1. Calculate ego-vehicle's footprint on pull out path between from current position to merge end point. (Illustrated by blue frame) +1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame) 2. Calculate object's polygon which is located in shoulder lane -3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.5 m`), that is judged as a unsafe path +3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path -![pull_out](./image/pull_out_fig1.drawio.svg) +![pull_out_collision_check](./image/pull_out_collision_check.drawio.svg) #### **Path Generation** -The path is generated with a certain margin from the left edge of the shoulder lane. +There are two path generation methods. + +##### **shift pull out** + +Pull out distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one that generates a safe path is selected. + +- Generate the shoulder lane centerline and shift it to the current position. +- In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) +- Combine this path with center line of road lane + +![shift_pull_out](./image/shift_pull_out.drawio.svg) + +[Video of how shift pull out works](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4) + +###### parameters for shift pull out + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| before_pull_out_straight_distance | [m] | double | distance before pull out start point | 0.0 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | + +##### **geometric pull out** + +Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot. +See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. + +![geometric_pull_out](./image/geometric_pull_out.drawio.svg) + +[Video of how geometric pull out works](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4) + +###### parameters for geometric pull out + +| Name | Unit | Type | Description | Default value | +| :-------------------------- | :---- | :----- | :------------------------------------------------------ | :------------ | +| enable_geometric_pull_out | - | bool | flag whether to enable geometric pull out | true | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | + +#### **backward pull out start point search** + +If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`). + +![pull_out_after_back](./image/pull_out_after_back.drawio.svg) + +[Video of how pull out after backward driving works](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------------------------- | :------------- | :----- | :-------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_back | - | bool | flag whether to search backward for start_point | true | +| enable_back | - | bool | In the case of `efficient_path`, use efficient paths even if the back distance is longer. | +| In case of `short_back_distance`, use a path with as short a back distance | efficient_path | +| max_back_distance | [m] | double | maximum back distance | 15.0 | +| backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | +| backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | #### Unimplemented parts / limitations for pull put -- Note that the current module's start judgment condition may incorrectly judge that the vehicle is parked on the shoulder, such as when stopping in front of an intersection. +- pull out from the right shoulder lane to the left lane is not allowed. +- The safety of the road lane is not judged + - Collision prediction is not performed for vehicles approaching from behind the road lane. ### Side Shift @@ -341,6 +513,8 @@ When multiple turn signal conditions are met, the turn signal with the smaller d This module depends on the external [BehaviorTreeCpp](https://github.com/BehaviorTree/BehaviorTree.CPP) library. +[[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) H. Vorobieva, S. Glaser, N. Minoiu-Enache, and S. Mammar, “Geometric path planning for automatic parallel parking in tiny spots”, IFAC Proceedings Volumes, vol. 45, no. 24, pp. 36–42, 2012. + ## Future extensions / Unimplemented parts - diff --git a/planning/behavior_path_planner/behavior_path_planner_lane_change.md b/planning/behavior_path_planner/behavior_path_planner_lane_change.md new file mode 100644 index 0000000000000..dbed99fc42e59 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_lane_change.md @@ -0,0 +1,426 @@ +# Lane Change + +The Lane Change module is activated when lane change is needed and can be safely executed. + +## Lane Change Requirement + +- During lane change request condition + - The ego-vehicle isn’t on a `preferred_lane`. + - There is neither intersection nor crosswalk on the path of the lane change +- lane change ready condition + - Path of the lane change does not collide with other dynamic objects (see the figure below) + - Lane change candidate path is approved by an operator. + +## Generating Lane Change Candidate Path + +The lane change candidate path is divided into two phases: preparation and lane-changing. The following figure illustrates each phase of the lane change candidate path. + +![lane-change-phases](./image/lane_change/lane_change-lane_change_phases.png) + +### Preparation phase + +The preparation trajectory is the candidate path's first and the straight portion generated along the ego vehicle's current lane. The length of the preparation trajectory is computed as follows. + +```C++ +lane_change_prepare_distance = max(current_speed * lane_change_prepare_duration + 0.5 * deceleration * lane_change_prepare_duration^2, minimum_lane_change_prepare_distance) +``` + +During the preparation phase, the turn signal will be activated when the remaining distance is equal to or less than `lane_change_search_distance`. + +### Lane-changing phase + +The lane-changing phase consist of the shifted path that moves ego from current lane to the target lane. Total distance of lane-changing phase is as follows. + +```C++ +lane_change_prepare_velocity = current_speed + deceleration * lane_change_prepare_duration +lane_changing_distance = max(lane_change_prepare_velocity * lane_changing_duration + 0.5 * deceleration * lane_changing_duration^2, minimum_lane_change_length + backward_length_buffer_for_end_of_lane) +``` + +The `backward_length_buffer_for_end_of_lane` is added to allow some window for any possible delay, such as control or mechanical delay during brake lag. + +#### Multiple candidate path samples + +Lane change velocity is affected by the ego vehicle's current velocity. High velocity requires longer preparation and lane changing distance. However we also need to plan lane changing trajectories in case ego vehicle slows down. +Computing candidate paths that assumes ego vehicle's slows down is performed by substituting predetermined deceleration value into `lane_change_prepare_distance`, `lane_change_prepare_velocity` and `lane_changing_distance` equation. + +The predetermined deceleration are a set of value that starts from `deceleration = 0.0`, and decrease by `acceleration_resolution` until it reaches`deceleration = -maximum_deceleration`. The `acceleration_resolution` is determine by the following + +```C++ +acceleration_resolution = maximum_deceleration / lane_change_sampling_num +``` + +The following figure illustrates when `lane_change_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. + +![path_samples](./image/lane_change/lane_change-candidate_path_samples.png) + +Which path will be chosen will depend on validity and collision check. + +#### Candidate Path's validity check + +A candidate path is valid if the total lane change distance is less than + +1. distance to the end of current lane +2. distance to the next intersection +3. distance from current pose to the goal. +4. distance to the crosswalk. + +The goal must also be in the list of the preferred lane. + +The following flow chart illustrates the validity check. + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title Selecting Valid Candidate Paths +start +:**INPUT** std::vector input_paths; + +partition selectValidPaths { +:**INITIALIZE** std::vector valid_paths; + +:idx = 0; + +while (idx < input_paths.size()?) + +:path = input_paths.at(idx); + +partition hasEnoughDistance { + +if(lane_change_total_distance < distance to end of current lanes +&& +lane_change_total_distance < distance to the next intersection +&& +lane_change_total_distance < distance from current pose to the goal +&& +lane_change_total_distance < distance to crosswalk +&& +goal is in route +) then (true) +:path_validity = true; +else (\n false) +:path_validity = false; +endif +} + +if(path_validity == true)then (true) + +:valid_paths.push_back(path); + +else (\nfalse) +endif +:++idx; +endwhile (false) + +:**RETURN** valid_paths; + +} +stop +@enduml +``` + +#### Candidate Path's Safety check + +Valid candidate path is evaluated for safety before is was selected as the output candidate path. The flow of the process is as follows. + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title Safe and Force Lane Change +start +:**INPUT** std::vector valid_paths; + +partition selectValidPaths { +:**INITIALIZE** std::vector valid_paths; + +:idx = 0; + +while (idx < input_paths.size()?) is (TRUE) + +:path = valid_paths.at(idx); + + +if(path pass safety check?) then (TRUE) +:selected_path = path, is_path_safe = true; +else (NO) +endif + +:++idx; +endwhile (FALSE) + +if(valid_paths.empty()?)then (true) + +:selected_path = valid_paths.front(), is_path_safe = false; +note left +used for +**FORCE LANE CHANGE** +if FORCE is needed, +then there is no safe path +end note +else (\nfalse) +endif + +:**RETURN** selected_path && is_path_safe; + +} +stop +@enduml + +``` + +If all valid candidate path is unsafe, then the operator will have the option to perform force lane change by using the front-most candidate path as the output. The force lane change will ignore all safety checks. + +A candidate path's is safe if it satisfies the following lateral distance criteria, + +```C++ +lateral distance > lateral_distance_threshold +``` + +However, suppose the lateral distance is insufficient. In that case, longitudinal distance will be evaluated. The candidate path is safe only when the longitudinal gap between the ego vehicle and the dynamic object is wide enough. + +The following charts illustrate the flow of the safety checks + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title Collision/Safety Check +start +:**INPUT** valid_path; + +:**CONVERT** valid_path **to** ego_predicted_path; + +:idx = 0; + +:is_safe_path = true; + +:objects = dynamic_objects in current and target lane; +while (idx < objects.size()?) is (TRUE) + +:obj = objects.at(idx); + +:**INITIALIZE** time = 0; + +:check_time = prepare_duration + lane_changing_duration; + +while(time == check_time?) is (FALSE) + +if(lateral distance is sufficient) then (TRUE) + +else +if (is ego in front of object?) then (YES) +:v_front = v_ego +v_rear = v_obj; +else (NO) +:v_front = v_obj +v_rear = v_ego; +endif +if(longitudinal distance is sufficient) then (YES) +else (NO) +:is_safe_path = false; +break +endif +endif + +:t+=prediction_time_resolution; + +endwhile (TRUE) + +if(if_safe_path == TRUE?) then (YES) +else (NO) +break; +endif + +:++idx; + +endwhile (FALSE) + +:**RETURN** is_safe_path; + +stop +@enduml + +``` + +##### Calculating and evaluating longitudinal distance + +A sufficient longitudinal gap between vehicles will prevent any rear-end collision from happening. This includes an emergency stop or sudden braking scenarios. + +The following information is required to evaluate the longitudinal distance between vehicles + +1. estimated speed of the dynamic objects +2. predicted path of dynamic objects +3. ego vehicle's current speed +4. ego vehicle's predicted path (converted/estimated from candidate path) + +The following figure illustrates how the safety check is performed on ego vs. dynamic objects. + +![Safety check](./image/lane_change/lane_change-collision_check.png) + +Let `v_front` and `a_front` be the front vehicle's velocity and deceleration, respectively, and `v_rear` and `a_rear` be the rear vehicle's velocity and deceleration, respectively. +Front vehicle and rear vehicle assignment will depend on which predicted path's pose is currently being evaluated. + +The following figure illustrates front and rear vehicle velocity assignment. + +![front rear assignment](./image/lane_change/lane_change-collision_check_parked_vehicle.png) + +Assuming the front vehicle brakes, then `d_front` is the distance the front vehicle will travel until it comes to a complete stop. The distance is computed from the equation of motion, which yield. + +```C++ +d_front = -std::pow(v_front,2) / (2*a_front) +``` + +Using the same formula to evaluate the rear vehicle's stopping distance `d_rear` is insufficient. This is because as the rear vehicle's driver saw the front vehicle's sudden brake, it will take some time for the driver to process the information and react by pressing the brake. We call this delay the reaction time. + +The reaction time is considered from the duration starting from the driver seeing the front vehicle brake light until the brake is pressed. As the brake is pressed, the time margin (which might be caused by mechanical or control delay) also needs to be considered. With these two parameters included, the formula for `d_rear` will be as follows. + +```C++ +d_rear = v_rear * rear_vehicle_reaction_time + v_rear * rear_vehicle_safety_time_margin + (-std::pow(v_front,2) / 2 * a_rear) +``` + +Since there is no absolute value for the deceleration`a_front` and `a_rear`, both of the values are parameterized (`expected_front_deceleration` and `expected_rear_deceleration`, respectively) with the estimation of how much deceleration will occur if the brake is pressed. + +The longitudinal distance is evaluated as follows + +```C++ +d_rear < d_front + d_inter +``` + +where `d_inter` is the relative longitudinal distance obtained at each evaluated predicted pose. + +Finally minimum longitudinal distance for `d_rear` is added to compensate for object to near to each other when `d_rear = 0.0`. This yields + +```C++ +std::max(longitudinal_distance_min_threshold, d_rear) < d_front + d_inter +``` + +#### If the lane is blocked and multiple lane changes + +When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance computation is as follows. + +```C++ +minimum_lane_change_distance = num_of_lane_changes * (minimum_lane_change_length + backward_length_buffer_for_end_of_lane) +``` + +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) + +### Abort lane change + +The lane changing process can still be aborted if the proper conditions are satisfied. The following states are validated before the abort request is approved. + +1. Ego's current speed is under the threshold, and ego is still in the current lane +2. ego relative distance from the centerline and ego relative yaw from lanelet's orientation is under the threshold. + +The following depicts the flow of the abort lane change check. + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center + +title Abort Lane Change +start +if (path if safe?) then (**NO**) + if (current velocity < abort_lane_change_velocity_thresh && ego is in current lane ?) then (**TRUE**) + :**ABORT**; + stop + else (**FALSE**) + if (distance of ego to centerline < abort_lane_change_distance_thresh + && + ego's current yaw with reference to current lane's orientation < abort_lane_change_angle_thresh) then (**TRUE**) + :**ABORT**; + stop + else (**FALSE**) + endif + endif +else (**YES**) +endif +:**DONT ABORT**; +stop +@enduml +``` + +## Parameters + +### Essential lane change parameters + +The following parameters are configurable in `behavior_path_planner.param.yaml`. + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------- | ---- | ------ | ------------------------------------------------------------------------------------- | ------------- | +| `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 | 5.0 | +| `minimum_lane_change_length` | [m] | double | The minimum distance needed for changing lanes. | 12.0 | + +The following parameters are configurable in `lane_change.param.yaml`. + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | ------- | ------- | --------------------------------------------------------------------------------------- | ------------- | +| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `lane_changing_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 5.6 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `maximum_deceleration` | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change. | 1.0 | +| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | +| `enable_abort_lane_change` | [-] | boolean | Enable abort lane change. (\*1) | false | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | false | +| `lane_change_search_distance` | [m] | double | The turn signal activation distance during the lane change preparation phase. | 30.0 | + +### Collision checks during lane change + +The following parameters are configurable in `behavior_path_planner.param.yaml`. + +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 5.0 | +| `longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*2) | -1.0 | +| `expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*2) | -1.0 | +| `rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `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 | + +(\*2) the value must be negative. + +### Abort lane change + +The following parameters are configurable in `lane_change.param.yaml`. + +| Name | Unit | Type | Description | Default value | +| :------------------------------------- | ----- | ------- | ------------------------------------------------------------------------------------ | ------------- | +| `enable_abort_lane_change` | [-] | boolean | Enable abort lane change. (\*1) | false | +| `abort_lane_change_velocity_thresh` | [m/s] | double | The velocity threshold to abort lane change while ego is still in current lane. | 0.5 | +| `abort_lane_change_angle_threshold` | [deg] | double | The angle threshold based on angle between ego's current yaw and lane's orientation. | 10.0 | +| `abort_lane_change_distance_threshold` | [m] | double | The distance threshold based on relative distance from ego pose to the centerline. | 0.3 | + +### Debug + +The following parameters are configurable in `lane_change.param.yaml`. + +| Name | Unit | Type | Description | Default value | +| :--------------------- | ---- | ------- | ---------------------------- | ------------- | +| `publish_debug_marker` | [-] | boolean | Flag to publish debug marker | false | + +## Debug Marker & Visualization + +The debug marker can be enable via `lane_change.param.yaml`. Simply set the `publish_debug_marker` to `true`, restart `rviz2` and add the marker `planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/markers/MarkerArray`. + +![debug](./image/lane_change/lane_change-debug.png) + +## Limitations 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 f0889cc638c02..0b643151aa03b 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -20,3 +20,10 @@ path_interval: 2.0 visualize_drivable_area_for_shared_linestrings_lanelet: true + + lateral_distance_max_threshold: 5.0 + longitudinal_distance_min_threshold: 3.0 + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 2.0 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 86226a95ce561..a1ad81b61e931 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 @@ -1,19 +1,19 @@ /**: ros__parameters: lane_change: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - lane_change_prepare_duration: 4.0 - lane_changing_duration: 8.0 - lane_change_finish_judge_buffer: 3.0 - minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 - prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - enable_abort_lane_change: false - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - enable_blocked_by_obstacle: false + lane_change_prepare_duration: 4.0 # [s] + lane_changing_duration: 8.0 # [s] + minimum_lane_change_prepare_distance: 4.0 # [m] + lane_change_finish_judge_buffer: 3.0 # [m] + minimum_lane_change_velocity: 5.6 # [m/s] + prediction_time_resolution: 0.5 # [s] + maximum_deceleration: 1.0 # [m/s2] + lane_change_sampling_num: 10 + abort_lane_change_velocity_thresh: 0.5 + abort_lane_change_angle_thresh: 10.0 # [deg] + abort_lane_change_distance_thresh: 0.3 # [m] + enable_abort_lane_change: true + enable_collision_check_at_prepare_phase: true + use_predicted_path_outside_lanelet: true + use_all_predicted_path: true + publish_debug_marker: false diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index 872e79340bd3b..7a9a63f805d67 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -1,26 +1,30 @@ /**: ros__parameters: pull_out: - min_stop_distance: 2.0 - stop_time: 0.0 - hysteresis_buffer_distance: 1.0 - pull_out_prepare_duration: 4.0 - pull_out_duration: 2.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 pull_out_finish_judge_buffer: 1.0 - minimum_pull_out_velocity: 2.0 - prediction_duration: 30.0 - prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - enable_abort_pull_out: false - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - use_dynamic_object: false - enable_blocked_by_obstacle: false - pull_out_search_distance: 30.0 - before_pull_out_straight_distance: 5.0 - after_pull_out_straight_distance: 5.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + before_pull_out_straight_distance: 0.0 + minimum_shift_pull_out_distance: 20.0 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 + # geometric pull out + enable_geometric_pull_out: true + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 15.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index ce33bfaed7382..d11ed1f0db74f 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -2,43 +2,45 @@ ros__parameters: pull_over: request_length: 100.0 - th_arrived_distance_m: 1.0 - th_stopped_velocity_mps: 0.01 - th_stopped_time_sec: 2.0 # It must be greater than the state_machine's. + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. pull_over_velocity: 3.0 pull_over_minimum_velocity: 0.3 margin_from_boundary: 0.5 decide_path_distance: 10.0 - min_acc: -0.5 - enable_shift_parking: true - enable_arc_forward_parking: true - enable_arc_backward_parking: true + maximum_deceleration: 0.5 # goal research - search_priority: "efficient_path" # "efficient_path" or "close_goal" enable_goal_research: true + search_priority: "efficient_path" # "efficient_path" or "close_goal" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 goal_search_interval: 1.0 goal_to_obj_margin: 2.0 # occupancy grid map - collision_check_margin: 0.0 + collision_check_margin: 0.5 theta_size: 360 obstacle_threshold: 60 # shift path + enable_shift_parking: true pull_over_sampling_num: 4 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - maximum_deceleration: 1.0 after_pull_over_straight_distance: 5.0 before_pull_over_straight_distance: 5.0 # parallel parking path + enable_arc_forward_parking: true + enable_arc_backward_parking: true after_forward_parking_straight_distance: 2.0 after_backward_parking_straight_distance: 2.0 forward_parking_velocity: 0.3 backward_parking_velocity: -0.3 + forward_parking_lane_departure_margin: 0.0 + backward_parking_lane_departure_margin: 0.0 arc_path_interval: 1.0 - # hazard. Not used now. + pull_over_max_steer_angle: 0.35 # 20deg + # hazard on when parked hazard_on_threshold_dis: 1.0 hazard_on_threshold_vel: 0.5 # check safety with dynamic objects. Not used now. diff --git a/planning/behavior_path_planner/image/arc_backward_parking.drawio.svg b/planning/behavior_path_planner/image/arc_backward_parking.drawio.svg new file mode 100644 index 0000000000000..eef5ba786bac3 --- /dev/null +++ b/planning/behavior_path_planner/image/arc_backward_parking.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull over end point
pull over end point
pull over start point
pull over start point
margin_from_boundary
margin_from_boundary
stop
stop
stop
stop
goal
goal
backward_parking_lane_departure_margin
backward_parking_lane_departure_marginText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/arc_forward_parking.drawio.svg b/planning/behavior_path_planner/image/arc_forward_parking.drawio.svg new file mode 100644 index 0000000000000..d058582e3af58 --- /dev/null +++ b/planning/behavior_path_planner/image/arc_forward_parking.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull over end point
pull over end point
pull over start point
pull over start point
margin_from_boundary
margin_from_boundary
goal
goal
stop
stop
stop
stop
stop
stop
forward_parking_lane_departure_margin
forward_parking_lane_departure_marginText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/behavior_path_planner_bt_config.png b/planning/behavior_path_planner/image/behavior_path_planner_bt_config.png index fddd962012e4a..72b6fbf36d119 100644 Binary files a/planning/behavior_path_planner/image/behavior_path_planner_bt_config.png and b/planning/behavior_path_planner/image/behavior_path_planner_bt_config.png differ diff --git a/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg b/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg new file mode 100644 index 0000000000000..802b6138d5f13 --- /dev/null +++ b/planning/behavior_path_planner/image/geometric_pull_out.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start poi...
stop
stopText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png b/planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png new file mode 100644 index 0000000000000..8578549bb1f5b Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-candidate_path_samples.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-collision_check.png b/planning/behavior_path_planner/image/lane_change/lane_change-collision_check.png new file mode 100644 index 0000000000000..7fc897471716e Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-collision_check.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-collision_check_parked_vehicle.png b/planning/behavior_path_planner/image/lane_change/lane_change-collision_check_parked_vehicle.png new file mode 100644 index 0000000000000..32ab7b34b7966 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-collision_check_parked_vehicle.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug.png b/planning/behavior_path_planner/image/lane_change/lane_change-debug.png new file mode 100644 index 0000000000000..501f03742f531 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-debug.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png b/planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png new file mode 100644 index 0000000000000..f5898c8f77727 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-intersection_case.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png b/planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png new file mode 100644 index 0000000000000..411a72c30b88a Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-lane_change_phases.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png b/planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png new file mode 100644 index 0000000000000..4df48d576a1ce Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-when_cannot_change_lanes.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg b/planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg new file mode 100644 index 0000000000000..a653a7d5f03f6 --- /dev/null +++ b/planning/behavior_path_planner/image/lane_change/lane_change.drawio.svg @@ -0,0 +1,4 @@ + + + +
lane change
finish judge buffer
lane change...
lane change prepare distance
lane change prepare di...
lane changing distance
lane changing dis...
$$v_{current}$$
$$v_{current}$$
linestrings with
allow_lane_change = true tags
linestrings with...
$$v_{prepare}$$
$$v_{prepare}$$
preparation phase
turn signal activation
preparation phase...
lane changing phase
lane changing phase
current_lane
current_lane
target_lane (preferred_lane)
target_lane (preferred...
buffer
buffer
$$v_{changing}$$
$$v_{changing}$$Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg b/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg new file mode 100644 index 0000000000000..f89ddde14db47 --- /dev/null +++ b/planning/behavior_path_planner/image/pull_out_after_back.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start point
stop
stop
stop
stop
backward_search_resolution
backward_search_resolutionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg b/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg new file mode 100644 index 0000000000000..53afb6db58db4 --- /dev/null +++ b/planning/behavior_path_planner/image/pull_out_collision_check.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start poi...
collision check
collision checkText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/pull_out_fig1.drawio.svg b/planning/behavior_path_planner/image/pull_out_fig1.drawio.svg deleted file mode 100644 index f629ce734c10e..0000000000000 --- a/planning/behavior_path_planner/image/pull_out_fig1.drawio.svg +++ /dev/null @@ -1 +0,0 @@ -
merge end point
merge end point
merge start point
merge start point
colision check
colision checkViewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/behavior_path_planner/image/pull_over_fig1.drawio.svg b/planning/behavior_path_planner/image/pull_over_fig1.drawio.svg deleted file mode 100644 index 131f9f6ca83c2..0000000000000 --- a/planning/behavior_path_planner/image/pull_over_fig1.drawio.svg +++ /dev/null @@ -1,340 +0,0 @@ - - - - - - - - - - - - -
-
-
- merge end point -
-
-
- - - merge end point - - - - - - - -
-
-
- merge start point -
-
-
- - - merge start point - - - - - - - - - - - - - -
-
-
- margin -
-
-
- - - margin - - - - - - - - - - - - - - - -
-
-
- goal -
-
-
- - - goal - - - - - - - - - - - - - - - - - -
-
-
- center line of shulder lane -
-
-
- - - center line of shulder lane - - - - - - - - - -
-
-
- uniform offset -
-
-
- - - uniform offset - - - - - - - - -
-
-
- shifted path -
-
-
- - - shifted path - - - - - - - - - - - - -
-
-
- center line of road lane -
-
-
- - - center line of road lane - - - - - - - - - - - - -
-
-
- merge end point -
-
-
- - - merge end point - - - - - - - -
-
-
- merge start point -
-
-
- - - merge start point - - - - - - - - - - - - - - - - - -
-
-
- goal -
-
-
- - - goal - - - - - - - - - - - - - - - -
-
-
- straight distance -
-
-
- - - straight distance - - - - - - - - -
-
-
- pull over distance -
-
-
- - - pull over distance - - - - - - - - - - -
-
-
- - combine - -
-
-
-
- - - combine - - - - - - - - - - - - -
-
-
- shifting length -
-
-
- - - shifting length - - - - - - - - - Viewer does not support full SVG 1.1 - - - - \ No newline at end of file diff --git a/planning/behavior_path_planner/image/shift_parking.drawio.svg b/planning/behavior_path_planner/image/shift_parking.drawio.svg new file mode 100644 index 0000000000000..d5ce6b392beb5 --- /dev/null +++ b/planning/behavior_path_planner/image/shift_parking.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull over end point
pull over end point
pull over start point
pull over start point
margin_from_boundary
margin_from_boundary
goal
goalText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_path_planner/image/shift_pull_out.drawio.svg b/planning/behavior_path_planner/image/shift_pull_out.drawio.svg new file mode 100644 index 0000000000000..887294bbe5d23 --- /dev/null +++ b/planning/behavior_path_planner/image/shift_pull_out.drawio.svg @@ -0,0 +1,4 @@ + + + +
pull out end point
pull out end point
pull out start point
pull out start poi...
before_pull_out_distance
before_pul...
pull_out_distance
pull_out_d...Text is not SVG - cannot display \ No newline at end of file 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 b06397c385f2f..2b062c5742fd5 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 @@ -46,6 +46,18 @@ #include #include +template +inline void update_param( + const std::vector & parameters, const std::string & name, T & value) +{ + const auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + value = static_cast(it->template get_value()); + } +} + namespace behavior_path_planner { using ApprovalMsg = tier4_planning_msgs::msg::Approval; @@ -59,6 +71,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; +using rcl_interfaces::msg::SetParametersResult; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::Scenario; @@ -96,6 +109,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool isDataReady(); // parameters + std::shared_ptr avoidance_param_ptr; + std::shared_ptr lane_change_param_ptr; + BehaviorPathPlannerParameters getCommonParam(); BehaviorTreeManagerParam getBehaviorTreeManagerParam(); SideShiftParameters getSideShiftParam(); @@ -113,14 +129,14 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onForceApproval(const PathChangeModule::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const HADMapRoute::ConstSharedPtr route_msg); + SetParametersResult onSetParam(const std::vector & parameters); /** * @brief Modify the path points near the goal to smoothly connect the lanelet and the goal point. */ PathWithLaneId modifyPathForSmoothGoalConnection( const PathWithLaneId & path) const; // (TODO) move to util - - void clipPathLength(PathWithLaneId & path) const; // (TODO) move to util + OnSetParametersCallbackHandle::SharedPtr m_set_param_res; /** * @brief Execute behavior tree and publish planned data. @@ -148,6 +164,28 @@ class BehaviorPathPlannerNode : public rclcpp::Node // debug rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; + /** + * @brief check path if it is unsafe or forced + */ + bool isForcedCandidatePath() const; + + template + size_t findEgoIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + p->parameters.ego_nearest_yaw_threshold); + } + + template + size_t findEgoSegmentIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + p->parameters.ego_nearest_yaw_threshold); + } }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp index 995b7e5385858..b36d02a880dde 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp @@ -15,10 +15,12 @@ #define BEHAVIOR_PATH_PLANNER__DEBUG_UTILITIES_HPP_ #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include #include +#include #include #include #include @@ -31,16 +33,52 @@ namespace marker_utils { 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::ShiftPointArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +struct CollisionCheckDebug +{ + std::string failed_reason; + std::size_t lane_id{0}; + Pose current_pose{}; + Twist current_twist{}; + Pose expected_ego_pose{}; + Pose expected_obj_pose{}; + Pose relative_to_ego{}; + bool is_front{false}; + bool allow_lane_change{false}; + std::vector lerped_path; + std::vector ego_predicted_path{}; + Polygon2d ego_polygon{}; + Polygon2d obj_polygon{}; +}; + +constexpr std::array, 10> colorsList() +{ + constexpr std::array red = {1., 0., 0.}; + constexpr std::array green = {0., 1., 0.}; + constexpr std::array blue = {0., 0., 1.}; + constexpr std::array yellow = {1., 1., 0.}; + constexpr std::array aqua = {0., 1., 1.}; + constexpr std::array magenta = {1., 0., 1.}; + constexpr std::array medium_orchid = {0.729, 0.333, 0.827}; + constexpr std::array light_pink = {1, 0.713, 0.756}; + constexpr std::array light_yellow = {1, 1, 0.878}; + constexpr std::array light_steel_blue = {0.690, 0.768, 0.870}; + return {red, green, blue, yellow, aqua, + magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; +} + inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } MarkerArray createPoseMarkerArray( 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 050bbaa616e1c..bd6e1f7a527bc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -41,6 +41,9 @@ struct BehaviorPathPlannerParameters double path_interval; + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + // vehicle info vehicle_info_util::VehicleInfo vehicle_info; double wheel_base; @@ -56,6 +59,14 @@ struct BehaviorPathPlannerParameters // drivable area visualization bool visualize_drivable_area_for_shared_linestrings_lanelet; + + // collision check + double lateral_distance_max_threshold; + double longitudinal_distance_min_threshold; + double expected_front_deceleration; + double expected_rear_deceleration; + 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/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp index f0568149540a0..dc05d30eb44a2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp @@ -41,17 +41,18 @@ std::vector calcPathArcLengthArray( const PathWithLaneId & path, size_t start = 0, size_t end = std::numeric_limits::max(), double offset = 0.0); -double calcPathArcLength( - const PathWithLaneId & path, size_t start = 0, size_t end = std::numeric_limits::max()); - PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval); Path toPath(const PathWithLaneId & input); -size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const double signed_arc); +size_t getIdxByArclength( + const PathWithLaneId & path, const size_t target_idx, const double signed_arc); + +void clipPathLength( + PathWithLaneId & path, const size_t target_idx, const double forward, const double backward); void clipPathLength( - PathWithLaneId & path, const Pose base_pose, const double forward, const double backward); + PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params); std::pair getPathTurnSignal( const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path, 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 e74a407b5a7e4..4a1628533988a 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 @@ -40,7 +40,7 @@ class AvoidanceModule : public SceneModuleInterface public: AvoidanceModule( - const std::string & name, rclcpp::Node & node, const AvoidanceParameters & parameters); + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters); bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -69,10 +69,8 @@ class AvoidanceModule : public SceneModuleInterface return false; } - void setParameters(const AvoidanceParameters & parameters); - private: - AvoidanceParameters parameters_; + std::shared_ptr parameters_; AvoidancePlanningData avoidance_data_; @@ -260,8 +258,6 @@ class AvoidanceModule : public SceneModuleInterface PathWithLaneId calcCenterLinePath( const std::shared_ptr & planner_data, const PoseStamped & pose) const; - void clipPathLength(PathWithLaneId & path) const; - // TODO(Horibe): think later. // for unique ID mutable uint64_t original_unique_id = 0; // TODO(Horibe) remove mutable diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp new file mode 100644 index 0000000000000..ae32cd130b574 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/debug.hpp @@ -0,0 +1,43 @@ +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ + +#include "behavior_path_planner/debug_utilities.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" + +#include +#include +#include + +namespace marker_utils::lane_change_markers +{ +using behavior_path_planner::LaneChangePath; +using marker_utils::CollisionCheckDebug; +using visualization_msgs::msg::MarkerArray; +MarkerArray showObjectInfo( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showAllValidLaneChangePath( + const std::vector & lanes, std::string && ns); +MarkerArray showLerpedPose( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showEgoPredictedPaths( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showPolygon( + const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showPolygonPose( + const std::unordered_map & obj_debug_vec, std::string && ns); +} // namespace marker_utils::lane_change_markers +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 7813661a31f02..0706eb64c493d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ +#include "behavior_path_planner/scene_module/lane_change/debug.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" @@ -24,26 +25,25 @@ #include +#include #include +#include #include #include namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using marker_utils::CollisionCheckDebug; struct LaneChangeParameters { - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; double lane_change_prepare_duration; double lane_changing_duration; + double minimum_lane_change_prepare_distance; double lane_change_finish_judge_buffer; double minimum_lane_change_velocity; - double prediction_duration; double prediction_time_resolution; - double static_obstacle_velocity_thresh; double maximum_deceleration; int lane_change_sampling_num; double abort_lane_change_velocity_thresh; @@ -53,7 +53,7 @@ struct LaneChangeParameters bool enable_collision_check_at_prepare_phase; bool use_predicted_path_outside_lanelet; bool use_all_predicted_path; - bool enable_blocked_by_obstacle; + bool publish_debug_marker; }; struct LaneChangeStatus @@ -72,7 +72,8 @@ class LaneChangeModule : public SceneModuleInterface { public: LaneChangeModule( - const std::string & name, rclcpp::Node & node, const LaneChangeParameters & parameters); + const std::string & name, rclcpp::Node & node, + std::shared_ptr parameters); BehaviorModuleOutput run() override; @@ -102,10 +103,8 @@ class LaneChangeModule : public SceneModuleInterface return false; } - void setParameters(const LaneChangeParameters & parameters); - private: - LaneChangeParameters parameters_; + std::shared_ptr parameters_; LaneChangeStatus status_; PathShifter path_shifter_; @@ -167,11 +166,14 @@ class LaneChangeModule : public SceneModuleInterface void updateLaneChangeStatus(); bool isSafe() const; - bool isLaneBlocked(const lanelet::ConstLanelets & lanes) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied() const; bool hasFinishedLaneChange() const; + + void setObjectDebugVisualization() const; + mutable std::unordered_map object_debug_; + mutable std::vector debug_valid_path_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 9846e36e3aaef..dcd9d97c3efc2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -29,27 +29,34 @@ #include #include +#include +#include #include -namespace behavior_path_planner -{ -namespace lane_change_utils +namespace behavior_path_planner::lane_change_utils { +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 geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_autoware_utils::Polygon2d; PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); +double getExpectedVelocityWhenDecelerate( + const double & current_velocity, const double & expected_acceleration, + const double & lane_change_prepare_duration); +double getDistanceWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration, + const double & minimum_distance); std::vector getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::LaneChangeParameters & parameter); + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter); std::vector selectValidPaths( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, @@ -59,23 +66,24 @@ bool selectSafePath( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, + const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & ros_parameters, - LaneChangePath * selected_path); + LaneChangePath * selected_path, + std::unordered_map & debug_data); bool isLaneChangePathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, - const behavior_path_planner::LaneChangeParameters & ros_parameters, const bool use_buffer = true, + const size_t current_seg_idx, const Twist & current_twist, + const BehaviorPathPlannerParameters & common_parameters, + const behavior_path_planner::LaneChangeParameters & lane_change_parameters, + std::unordered_map & debug_data, const bool use_buffer = true, const double acceleration = 0.0); bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs); -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); -} // namespace lane_change_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp new file mode 100644 index 0000000000000..73f3ae83c0f6c --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp @@ -0,0 +1,41 @@ +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ + +#include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp" +#include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp" + +#include + +namespace behavior_path_planner +{ +class GeometricPullOut : public PullOutPlannerBase +{ +public: + explicit GeometricPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + const ParallelParkingParameters & parallel_parking_parameters); + + PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; + boost::optional plan(Pose start_pose, Pose goal_pose) override; + + GeometricParallelParking planner_; + ParallelParkingParameters parallel_parking_parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 42004374c1bb6..fd94ef35cd4ef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -15,14 +15,25 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_ +#include "behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" +#include +#include +#include #include +#include #include +#include + +#include #include #include #include @@ -30,48 +41,22 @@ namespace behavior_path_planner { -struct PullOutParameters -{ - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; - double pull_out_prepare_duration; - double pull_out_duration; - double pull_out_finish_judge_buffer; - double minimum_pull_out_velocity; - double prediction_duration; - double prediction_time_resolution; - double static_obstacle_velocity_thresh; - double maximum_deceleration; - int pull_out_sampling_num; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - bool use_dynamic_object; - bool enable_blocked_by_obstacle; - double pull_out_search_distance; - double before_pull_out_straight_distance; - double after_pull_out_straight_distance; - double maximum_lateral_jerk; - double minimum_lateral_jerk; - double deceleration_interval; -}; +using geometry_msgs::msg::PoseArray; +using lane_departure_checker::LaneDepartureChecker; struct PullOutStatus { - PathWithLaneId lane_follow_path; PullOutPath pull_out_path; - PullOutPath retreat_path; - PullOutPath straight_back_path; + 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 lane_follow_lane_ids; std::vector pull_out_lane_ids; - bool is_safe; - double start_distance; - bool back_finished; - bool is_retreat_path_valid; - Pose backed_pose; + bool is_safe = false; + bool back_finished = false; + Pose pull_out_start_pose; }; class PullOutModule : public SceneModuleInterface @@ -91,44 +76,50 @@ class PullOutModule : public SceneModuleInterface void onEntry() override; void onExit() override; - void setParameters(const PullOutParameters & parameters); + void setParameters(const PullOutParameters & parameters) { parameters_ = parameters; } + void resetStatus(); private: PullOutParameters parameters_; + vehicle_info_util::VehicleInfo vehicle_info_; + + std::vector> pull_out_planners_; PullOutStatus status_; - double pull_out_lane_length_ = 200.0; - double check_distance_ = 100.0; + std::vector backed_pose_candidates_; + PoseStamped backed_pose_; + std::deque odometry_buffer_; - PathWithLaneId getReferencePath() const; + std::unique_ptr last_route_received_time_; + std::unique_ptr last_pull_out_start_update_time_; + + std::shared_ptr getCurrentPlanner() const; lanelet::ConstLanelets getCurrentLanes() const; - lanelet::ConstLanelets getPullOutLanes(const lanelet::ConstLanelets & current_lanes) const; - std::pair getSafePath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - PullOutPath & safe_path) const; - std::pair getSafeRetreatPath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - RetreatPath & safe_backed_path, double & back_distance) const; - - bool getBackDistance( - const lanelet::ConstLanelets & pullover_lanes, const double check_distance, - PullOutPath & safe_path, double & back_distance) const; + PathWithLaneId getFullPath() const; + ParallelParkingParameters getGeometricPullOutParameters() const; + std::vector searchBackedPoses(); - // turn signal - TurnSignalInfo calcTurnSignalInfo(const ShiftPoint & shift_point) const; + std::shared_ptr lane_departure_checker_; + // turn signal + TurnSignalInfo calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const; + + void incrementPathIndex(); + PathWithLaneId getCurrentPath() const; + void planWithPriorityOnEfficientPath( + const std::vector & start_pose_candidates, const Pose & goal_pose); + void planWithPriorityOnShortBackDistance( + const std::vector & start_pose_candidates, const Pose & goal_pose); void updatePullOutStatus(); - bool isInLane( + static bool isInLane( const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint) const; - bool isLongEnough(const lanelet::ConstLanelets & lanelets) const; - bool isSafe() const; - bool isNearEndOfLane() const; - bool isCurrentSpeedLow() const; + const tier4_autoware_utils::LinearRing2d & vehicle_footprint); bool hasFinishedPullOut() const; - bool hasFinishedBack() const; - vehicle_info_util::VehicleInfo getVehicleInfo( - const BehaviorPathPlannerParameters & parameters) const; + void checkBackFinished(); + bool isStopped(); + bool hasFinishedCurrentPath(); + + void setDebugData() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp new file mode 100644 index 0000000000000..a994d57ee1368 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -0,0 +1,56 @@ + +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ + +#include + +namespace behavior_path_planner +{ +struct PullOutParameters +{ + double th_arrived_distance; + double th_stopped_velocity; + double th_stopped_time; + double collision_check_margin; + double pull_out_finish_judge_buffer; + // shift pull out + bool enable_shift_pull_out; + double shift_pull_out_velocity; + int pull_out_sampling_num; + double before_pull_out_straight_distance; + double minimum_shift_pull_out_distance; + double maximum_lateral_jerk; + double minimum_lateral_jerk; + double deceleration_interval; + // geometric pull out + bool enable_geometric_pull_out; + double geometric_pull_out_velocity; + double arc_path_interval; + double lane_departure_margin; + double backward_velocity; + double pull_out_max_steer_angle; + // search start pose backward + std::string search_priority; // "efficient_path" or "short_back_distance" + bool enable_back; + double max_back_distance; + double backward_search_resolution; + double backward_path_update_duration; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp index 5b32ff64babba..3f49dd82b8b29 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp @@ -19,23 +19,16 @@ #include +#include + namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; struct PullOutPath { - PathWithLaneId path; - ShiftedPath shifted_path; - ShiftPoint shift_point; - double acceleration = 0.0; - double preparation_length = 0.0; - double pull_out_length = 0.0; -}; - -struct RetreatPath -{ - behavior_path_planner::PullOutPath pull_out_path; - Pose backed_pose; + std::vector partial_paths; + Pose start_pose; + Pose end_pose; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp new file mode 100644 index 0000000000000..8144373c1e8a6 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp @@ -0,0 +1,71 @@ +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/parameters.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/util/create_vehicle_footprint.hpp" + +#include +#include + +#include + +#include +#include + +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::LinearRing2d; + +namespace behavior_path_planner +{ +enum class PlannerType { + NONE = 0, + SHIFT = 1, + GEOMETRIC = 2, +}; + +class PullOutPlannerBase +{ +public: + explicit PullOutPlannerBase(rclcpp::Node & node, const PullOutParameters & parameters) + { + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + parameters_ = parameters; + } + virtual ~PullOutPlannerBase() = default; + + void setPlannerData(std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + + virtual PlannerType getPlannerType() = 0; + virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + +protected: + std::shared_ptr planner_data_; + vehicle_info_util::VehicleInfo vehicle_info_; + LinearRing2d vehicle_footprint_; + PullOutParameters parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp new file mode 100644 index 0000000000000..4f43bb080fabd --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp @@ -0,0 +1,56 @@ +// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__SHIFT_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__SHIFT_PULL_OUT_HPP_ + +#include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/scene_module/pull_out/pull_out_planner_base.hpp" + +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using lane_departure_checker::LaneDepartureChecker; + +class ShiftPullOut : public PullOutPlannerBase +{ +public: + explicit ShiftPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + std::shared_ptr & lane_departure_checker); + + PlannerType getPlannerType() override { return PlannerType::SHIFT; }; + boost::optional plan(Pose start_pose, Pose goal_pose) override; + + std::vector calcPullOutPaths( + const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & common_parameter, + const behavior_path_planner::PullOutParameters & 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); + + std::shared_ptr lane_departure_checker_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp index 786686d950ab9..5107d875bcdd1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp @@ -37,49 +37,14 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); -std::vector getPullOutPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::PullOutParameters & parameter, const bool is_retreat_path = false); - -PullOutPath getBackPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::PullOutParameters & parameter, const double back_distance); - -bool isPathInLanelets4pullover( - const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets); - +PathWithLaneId getBackwardPath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const Pose & backed_pose, const double velocity); +lanelet::ConstLanelets getPullOutLanes( + const lanelet::ConstLanelets & current_lanes, + const std::shared_ptr & planner_data); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); - -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose); -bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, - const behavior_path_planner::PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, PullOutPath * selected_path); -bool isPullOutPathSafe( - const behavior_path_planner::PullOutPath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const behavior_path_planner::PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const bool use_buffer = true, - const bool use_dynamic_object = false); -bool hasEnoughDistance( - const PullOutPath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - const lanelet::routing::RoutingGraphContainer & overall_graphs); -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); } // namespace behavior_path_planner::pull_out_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index baa904c974bb1..0159c1a5956fe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -43,15 +43,12 @@ using visualization_msgs::msg::MarkerArray; struct PullOverParameters { double request_length; - double th_arrived_distance_m; - double th_stopped_velocity_mps; - double th_stopped_time_sec; + double th_arrived_distance; + double th_stopped_velocity; + double th_stopped_time; double margin_from_boundary; double decide_path_distance; - double min_acc; - bool enable_shift_parking; - bool enable_arc_forward_parking; - bool enable_arc_backward_parking; + double maximum_deceleration; // goal research std::string search_priority; // "efficient_path" or "close_goal" bool enable_goal_research; @@ -64,22 +61,27 @@ struct PullOverParameters double theta_size; double obstacle_threshold; // shift path + bool enable_shift_parking; int pull_over_sampling_num; double maximum_lateral_jerk; double minimum_lateral_jerk; double deceleration_interval; double pull_over_velocity; double pull_over_minimum_velocity; - double maximum_deceleration; double after_pull_over_straight_distance; double before_pull_over_straight_distance; // parallel parking + bool enable_arc_forward_parking; + bool enable_arc_backward_parking; double after_forward_parking_straight_distance; double after_backward_parking_straight_distance; double forward_parking_velocity; double backward_parking_velocity; + double forward_parking_lane_departure_margin; + double backward_parking_lane_departure_margin; double arc_path_interval; - // hazard. Not used now. + double pull_over_max_steer_angle; + // hazard double hazard_on_threshold_dis; double hazard_on_threshold_vel; // check safety with dynamic objects. Not used now. @@ -106,12 +108,14 @@ enum PathType { struct PUllOverStatus { PathWithLaneId path; + std::shared_ptr prev_stop_path = nullptr; lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets pull_over_lanes; lanelet::ConstLanelets lanes; // current + pull_over bool has_decided_path = false; int path_type = PathType::NONE; bool is_safe = false; + bool prev_is_safe = false; bool has_decided_velocity = false; bool has_requested_approval_ = false; }; @@ -185,18 +189,19 @@ class PullOverModule : public SceneModuleInterface std::unique_ptr last_approved_time_; PathWithLaneId getReferencePath() const; - PathWithLaneId getStopPath() const; + PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPullOverLanes() const; std::pair getSafePath(ShiftParkingPath & safe_path) const; Pose getRefinedGoal() const; Pose getParkingStartPose() const; - bool isLongEnoughToParkingStart(const PathWithLaneId path, const Pose parking_start_pose) const; + ParallelParkingParameters getGeometricPullOverParameters() const; + bool isLongEnoughToParkingStart( + const PathWithLaneId & path, const Pose & parking_start_pose) const; bool isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose goal_pose, const double buffer = 0) const; + const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const; bool isArcPath() const; double calcMinimumShiftPathDistance() const; double calcDistanceToPathChange() const; - bool planShiftPath(); bool isStopped(); bool hasFinishedCurrentPath(); @@ -210,7 +215,7 @@ class PullOverModule : public SceneModuleInterface std::pair getTurnInfo() const; // debug - Marker createParkingAreaMarker(const Pose back_pose, const Pose front_pose, const int32_t id); + Marker createParkingAreaMarker(const Pose & back_pose, const Pose & front_pose, const int32_t id); void publishDebugData(); void printParkingPositionError() const; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp index a2410320b86dc..0cc6eb6ebdb9e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp @@ -65,7 +65,7 @@ bool isPullOverPathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, + const size_t current_seg_idx, const Twist & current_twist, const double vehicle_width, const behavior_path_planner::PullOverParameters & ros_parameters, const bool use_buffer = true, const double acceleration = 0.0); bool hasEnoughDistance( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp index f8a29751862a4..5838d775c39be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp @@ -29,6 +29,7 @@ struct SceneModuleStatus explicit SceneModuleStatus(const std::string & n) : module_name(n) {} std::string module_name; // TODO(Horibe) should be const bool is_requested{false}; + bool is_execution_ready{false}; bool is_waiting_approval{false}; BT::NodeStatus status{BT::NodeStatus::IDLE}; }; 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 9be0be6ee6746..f5af0308c22da 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 @@ -36,6 +36,7 @@ #include #include #include +#include namespace behavior_path_planner { @@ -281,6 +282,24 @@ class SceneModuleInterface return uuid; } + template + size_t findEgoIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + p->parameters.ego_nearest_yaw_threshold); + } + + template + size_t findEgoSegmentIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->self_pose->pose, p->parameters.ego_nearest_dist_threshold, + p->parameters.ego_nearest_yaw_threshold); + } + public: BT::NodeStatus current_state_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index eb32326adc1f2..a8f60036d7554 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -48,26 +48,39 @@ using geometry_msgs::msg::PoseStamped; struct ParallelParkingParameters { - double th_arrived_distance_m; - double th_stopped_velocity_mps; + double th_arrived_distance; + double th_stopped_velocity; double after_forward_parking_straight_distance; double after_backward_parking_straight_distance; double forward_parking_velocity; double backward_parking_velocity; + double departing_velocity; + double backward_parking_lane_departure_margin; + double forward_parking_lane_departure_margin; + double departing_lane_departure_margin; double arc_path_interval; - double min_acc; + double maximum_deceleration; + double max_steer_angle; }; class GeometricParallelParking { public: bool isParking() const; - bool plan(const Pose goal_pose, lanelet::ConstLanelets lanes, const bool is_forward); - void setParams( - const std::shared_ptr & planner_data, ParallelParkingParameters parameters); + bool planPullOver( + const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanesconst, const bool is_forward); + bool planPullOut( + const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes); + void setData( + const std::shared_ptr & planner_data, + const ParallelParkingParameters & parameters); void incrementPathIndex(); - void clear(); + std::vector getArcPaths() const { return arc_paths_; } + std::vector getPaths() const { return paths_; } + PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; PathWithLaneId getArcPath() const; @@ -75,36 +88,40 @@ class GeometricParallelParking PoseStamped getCl() const { return Cl_; } PoseStamped getStartPose() const { return start_pose_; } PoseStamped getArcEndPose() const { return arc_end_pose_; } - PoseArray getPathPoseArray() const { return path_pose_array_; } private: std::shared_ptr planner_data_; ParallelParkingParameters parameters_; - // todo: use vehicle info after merging - // https://github.com/autowarefoundation/autoware.universe/pull/740 - float max_steer_deg_ = 40.0; // max steering angle [deg]. - float max_steer_rad_; - float R_E_min_; // base_link - float R_Bl_min_; // front_left + double R_E_min_; // base_link + double R_Bl_min_; // front_left + std::vector arc_paths_; std::vector paths_; size_t current_path_idx_ = 0; - bool planOneTraial( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, - const lanelet::ConstLanelets lanes, const bool is_forward); + void clearPaths(); + bool isEnoughDistanceToStart(const Pose & start_pose) const; + std::vector planOneTrial( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanesconst, + bool is_forward, const double end_pose_offset, const double lane_departure_margin); PathWithLaneId generateArcPath( - const Pose & center, const float radius, const float start_rad, float end_rad, + const Pose & center, const double radius, const double start_yaw, double end_yaw, const bool is_left_turn, const bool is_forward); PathPointWithLaneId generateArcPathPoint( - const Pose & center, const float radius, const float yaw, const bool is_left_turn, + const Pose & center, const double radius, const double yaw, const bool is_left_turn, const bool is_forward); - Pose calcStartPose( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, + boost::optional calcStartPose( + const Pose & goal_pose, const double start_pose_offset, const double R_E_r, const bool is_forward); - void generateStraightPath(const Pose start_pose); + std::vector generatePullOverPaths( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const double end_pose_offset, const double velocity); + PathWithLaneId generateStraightPath(const Pose & start_pose); + void setVelocityToArcPaths(std::vector & arc_paths, const double velocity); PoseStamped Cr_; PoseStamped Cl_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp index ad4c48103ecdf..ea489fc0468a6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp @@ -96,7 +96,7 @@ class PathShifter * @details The previous offset information is stored in the base_offset_. * This should be called after generate(). */ - void removeBehindShiftPointAndSetBaseOffset(const Point & base); + void removeBehindShiftPointAndSetBaseOffset(const Pose & pose, const size_t nearest_idx); //////////////////////////////////////// // Utility Functions @@ -169,13 +169,6 @@ class PathShifter */ std::vector calcLateralJerk(); - /** - * @brief Calculate shift point from path arclength for start and end point. - */ - static bool calcShiftPointFromArcLength( - const PathWithLaneId & path, const Point & origin, double dist_to_start, double dist_to_end, - double shift_length, ShiftPoint * shift_point); - private: // The reference path along which the shift will be performed. PathWithLaneId reference_path_; @@ -190,6 +183,7 @@ class PathShifter bool is_index_aligned_{false}; rclcpp::Logger logger_{rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; /** * @brief Calculate path index for shift_points and set is_index_aligned_ to true. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index 7ae9220541f0e..3203b46b17454 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -35,8 +35,9 @@ class TurnSignalDecider { public: TurnIndicatorsCommand getTurnSignal( - const PathWithLaneId & path, const Pose & current_pose, const RouteHandler & route_handler, - const TurnIndicatorsCommand & turn_signal_plan, const double plan_distance) const; + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const RouteHandler & route_handler, const TurnIndicatorsCommand & turn_signal_plan, + const double plan_distance) const; void setParameters(const double base_link2front, const double intersection_search_distance) { @@ -46,7 +47,7 @@ class TurnSignalDecider private: std::pair getIntersectionTurnSignal( - const PathWithLaneId & path, const Pose & current_pose, + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const RouteHandler & route_handler) const; rclcpp::Logger logger_{ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index bd88c63163f87..a3267ba1602b6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/debug_utilities.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" #include @@ -54,6 +55,8 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; + +using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -66,7 +69,11 @@ using geometry_msgs::msg::Vector3; using nav_msgs::msg::OccupancyGrid; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +namespace bg = boost::geometry; +using geometry_msgs::msg::Pose; +using marker_utils::CollisionCheckDebug; struct FrenetCoordinate3d { @@ -74,25 +81,55 @@ struct FrenetCoordinate3d double distance{0.0}; // lateral }; +struct ProjectedDistancePoint +{ + Point2d projected_point; + double distance{0.0}; +}; + +template > +ProjectedDistancePoint pointToSegment( + const Point2d & reference_point, const Point2d & point_from_ego, + const Point2d & point_from_object); + +void getProjectedDistancePointFromPolygons( + const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, + Pose & point_on_object); // data conversions Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id); -std::vector convertToPointArray(const PathWithLaneId & path); +std::vector convertToPoseArray(const PathWithLaneId & path); std::vector convertToGeometryPointArray(const PathWithLaneId & path); PoseArray convertToGeometryPoseArray(const PathWithLaneId & path); PredictedPath convertToPredictedPath( - const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const double duration, const double resolution, const double acceleration); + const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose, + const double nearest_seg_idx, const double duration, const double resolution, + const double acceleration, const double min_speed = 1.0); +template FrenetCoordinate3d convertToFrenetCoordinate3d( - const std::vector & linestring, const Point & search_point_geom); + const std::vector & pose_array, const Point & search_point_geom, const size_t seg_idx) +{ + FrenetCoordinate3d frenet_coordinate; -FrenetCoordinate3d convertToFrenetCoordinate3d( - const PathWithLaneId & path, const Point & search_point_geom); + const double longitudinal_length = + motion_utils::calcLongitudinalOffsetToSegment(pose_array, seg_idx, search_point_geom); + frenet_coordinate.length = + motion_utils::calcSignedArcLength(pose_array, 0, seg_idx) + longitudinal_length; + frenet_coordinate.distance = motion_utils::calcLateralOffset(pose_array, search_point_geom); + + return frenet_coordinate; +} + +inline FrenetCoordinate3d convertToFrenetCoordinate3d( + const PathWithLaneId & path, const Point & search_point_geom, const size_t seg_idx) +{ + return convertToFrenetCoordinate3d(path.points, search_point_geom, seg_idx); +} std::vector getIds(const lanelet::ConstLanelets & lanelets); @@ -120,13 +157,44 @@ double getArcLengthToTargetLanelet( Pose lerpByPose(const Pose & p1, const Pose & p2, const double t); -Point lerpByLength(const std::vector & array, const double length); +inline Point lerpByPoint(const Point & p1, const Point & p2, const double t) +{ + tf2::Vector3 v1, v2; + v1.setValue(p1.x, p1.y, p1.z); + v2.setValue(p2.x, p2.y, p2.z); -bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_pt); + const auto lerped_point = v1.lerp(v2, t); + + Point point; + point.x = lerped_point.x(); + point.y = lerped_point.y(); + point.z = lerped_point.z(); + return point; +} + +template +Point lerpByLength(const std::vector & point_array, const double length) +{ + Point lerped_point; + if (point_array.empty()) { + return lerped_point; + } + Point prev_geom_pt = tier4_autoware_utils::getPoint(point_array.front()); + double accumulated_length = 0; + for (const auto & pt : point_array) { + const auto & geom_pt = tier4_autoware_utils::getPoint(pt); + const double distance = tier4_autoware_utils::calcDistance3d(prev_geom_pt, geom_pt); + if (accumulated_length + distance > length) { + return lerpByPoint(prev_geom_pt, geom_pt, (length - accumulated_length) / distance); + } + accumulated_length += distance; + prev_geom_pt = geom_pt; + } + + return tier4_autoware_utils::getPoint(point_array.back()); +} -bool lerpByDistance( - const behavior_path_planner::PullOutPath & path, const double & s, Pose * lerped_pt, - const lanelet::ConstLanelets & road_lanes); +bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_pt); bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon); @@ -141,16 +209,27 @@ double getDistanceBetweenPredictedPathAndObject( const PredictedObject & object, const PredictedPath & path, const double start_time, const double end_time, const double resolution); -double getDistanceBetweenPredictedPathAndObjectPolygon( - const PredictedObject & object, const PullOutPath & ego_path, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, - const lanelet::ConstLanelets & road_lanes); +/** + * @brief Check collision between ego path footprints and objects. + * @return Has collision or not + */ +bool checkCollisionBetweenPathFootprintsAndObjects( + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, + const PredictedObjects & dynamic_objects, const double margin); + +/** + * @brief Check collision between ego footprints and objects. + * @return Has collision or not + */ +bool checkCollisionBetweenFootprintAndObjects( + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, + const PredictedObjects & dynamic_objects, const double margin); /** * @brief Get index of the obstacles inside the lanelets with start and end length * @return Indices corresponding to the obstacle inside the lanelets */ -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & lanelets, const double start_arc_length, const double end_arc_length); @@ -158,10 +237,13 @@ std::vector filterObjectsByLanelets( * @brief Get index of the obstacles inside the lanelets * @return Indices corresponding to the obstacle inside the lanelets */ -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); -std::vector filterObjectsByPath( +PredictedObjects filterObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + +std::vector filterObjectsIndicesByPath( const PredictedObjects & objects, const std::vector & object_indices, const PathWithLaneId & ego_path, const double vehicle_width); @@ -285,7 +367,69 @@ lanelet::ConstLanelets getExtendedCurrentLanes( lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length); +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & length, const double & width); + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const Shape & obj_shape); + +Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape); + +std::string getUuidStr(const PredictedObject & obj); + +std::vector getPredictedPathFromObj( + const PredictedObject & obj, const bool & is_use_all_predicted_path); + +Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object); + +bool getEgoExpectedPoseAndConvertToPolygon( + const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose, + tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time, + const double & length, const double & width); + +bool getObjectExpectedPoseAndConvertToPolygon( + const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose, + Polygon2d & obj_polygon, const double & check_current_time); + +bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); + +bool isObjectFront(const Pose & projected_ego_pose); + +double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel); + +double frontVehicleStopDistance( + const double & front_vehicle_velocity, const double & front_vehicle_accel, + const double & distance_to_collision); + +double rearVehicleStopDistance( + const double & rear_vehicle_velocity, const double & rear_vehicle_accel, + const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin); + +bool isLongitudinalDistanceEnough( + const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold); + +bool hasEnoughDistance( + const Pose & expected_ego_pose, const Twist & ego_current_twist, + const Pose & expected_object_pose, const Twist & object_current_twist, + const BehaviorPathPlannerParameters & param, CollisionCheckDebug & debug); + +bool isLateralDistanceEnough( + const double & relative_lateral_distance, const double & lateral_distance_threshold); + +bool isSafeInLaneletCollisionCheck( + const Pose & ego_current_pose, const Twist & ego_current_twist, + const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, + const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const double & check_time_resolution, const PredictedObject & target_object, + const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, + CollisionCheckDebug & debug); +bool isSafeInFreeSpaceCollisionCheck( + const Pose & ego_current_pose, const Twist & ego_current_twist, + const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, + const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const double & check_time_resolution, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 85dccb3dc2c0a..29c105f5e0871 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -4,12 +4,31 @@ behavior_path_planner 0.1.0 The behavior_path_planner package + + + Zulfaqar Azmi + + Kosuke Takeuchi + + Kosuke Takeuchi + + Fumiya Watanabe + Takamasa Horibe + + Yutaka Shimizu + + Takayuki Murooka + Apache License 2.0 Takamasa Horibe - mitsudome-r Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Yutaka Shimizu + Takayuki Murooka + Ryohsuke Mitsudome ament_cmake_auto 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 d962e36886d35..93e05415595e8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -101,6 +101,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1), createSubscriptionOptions(this)); + avoidance_param_ptr = std::make_shared(getAvoidanceParam()); + lane_change_param_ptr = std::make_shared(getLaneChangeParam()); + + m_set_param_res = this->add_on_set_parameters_callback( + std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); // behavior tree manager { mutex_bt_.lock(); @@ -112,17 +117,15 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_->registerSceneModule(side_shift_module); auto avoidance_module = - std::make_shared("Avoidance", *this, getAvoidanceParam()); + std::make_shared("Avoidance", *this, avoidance_param_ptr); bt_manager_->registerSceneModule(avoidance_module); auto lane_following_module = std::make_shared("LaneFollowing", *this, getLaneFollowingParam()); bt_manager_->registerSceneModule(lane_following_module); - const auto lane_change_param = getLaneChangeParam(); - auto lane_change_module = - std::make_shared("LaneChange", *this, lane_change_param); + std::make_shared("LaneChange", *this, lane_change_param_ptr); bt_manager_->registerSceneModule(lane_change_module); auto pull_over_module = std::make_shared("PullOver", *this, getPullOverParam()); @@ -203,7 +206,16 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.path_interval = declare_parameter("path_interval"); p.visualize_drivable_area_for_shared_linestrings_lanelet = declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); - + 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", 3.0); + p.longitudinal_distance_min_threshold = + declare_parameter("longitudinal_distance_min_threshold", 3.0); + p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0); + p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0); + p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 2.0); + p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0); return p; } @@ -307,27 +319,23 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() }; LaneChangeParameters p{}; - p.min_stop_distance = dp("min_stop_distance", 5.0); - p.stop_time = dp("stop_time", 2.0); - p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0); p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0); p.lane_changing_duration = dp("lane_changing_duration", 4.0); + p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0); p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0); p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 8.3); - p.prediction_duration = dp("prediction_duration", 8.0); p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); - p.static_obstacle_velocity_thresh = dp("static_obstacle_velocity_thresh", 0.1); p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.lane_change_sampling_num = dp("lane_change_sampling_num", 10); - p.enable_abort_lane_change = dp("enable_abort_lane_change", true); - p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); - p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); - p.use_all_predicted_path = dp("use_all_predicted_path", false); p.abort_lane_change_velocity_thresh = dp("abort_lane_change_velocity_thresh", 0.5); p.abort_lane_change_angle_thresh = dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); - p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false); + p.enable_abort_lane_change = dp("enable_abort_lane_change", true); + p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); + p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); + p.use_all_predicted_path = dp("use_all_predicted_path", true); + p.publish_debug_marker = dp("publish_debug_marker", false); // validation of parameters if (p.lane_change_sampling_num < 1) { @@ -357,18 +365,15 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() PullOverParameters p; p.request_length = dp("request_length", 100.0); - p.th_stopped_velocity_mps = dp("th_stopped_velocity_mps", 0.01); - p.th_arrived_distance_m = dp("th_arrived_distance_m", 0.3); - p.th_stopped_time_sec = dp("th_stopped_time_sec", 2.0); + p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); + p.th_arrived_distance = dp("th_arrived_distance", 0.3); + p.th_stopped_time = dp("th_stopped_time", 2.0); p.margin_from_boundary = dp("margin_from_boundary", 0.3); p.decide_path_distance = dp("decide_path_distance", 10.0); - p.min_acc = dp("min_acc", -0.5); - p.enable_shift_parking = dp("enable_shift_parking", true); - p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); - p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); + p.maximum_deceleration = dp("maximum_deceleration", 0.5); // goal research - p.search_priority = dp("search_priority", "efficient_path"); p.enable_goal_research = dp("enable_goal_research", true); + p.search_priority = dp("search_priority", "efficient_path"); p.forward_goal_search_length = dp("forward_goal_search_length", 20.0); p.backward_goal_search_length = dp("backward_goal_search_length", 20.0); p.goal_search_interval = dp("goal_search_interval", 5.0); @@ -378,21 +383,26 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.theta_size = dp("theta_size", 360); p.obstacle_threshold = dp("obstacle_threshold", 90); // shift path + p.enable_shift_parking = dp("enable_shift_parking", true); p.pull_over_sampling_num = dp("pull_over_sampling_num", 4); p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); p.deceleration_interval = dp("deceleration_interval", 10.0); p.pull_over_velocity = dp("pull_over_velocity", 8.3); p.pull_over_minimum_velocity = dp("pull_over_minimum_velocity", 0.3); - p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.after_pull_over_straight_distance = dp("after_pull_over_straight_distance", 3.0); p.before_pull_over_straight_distance = dp("before_pull_over_straight_distance", 3.0); // parallel parking + p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); + p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); p.after_forward_parking_straight_distance = dp("after_forward_parking_straight_distance", 0.5); p.after_backward_parking_straight_distance = dp("after_backward_parking_straight_distance", 0.5); p.forward_parking_velocity = dp("forward_parking_velocity", 1.0); p.backward_parking_velocity = dp("backward_parking_velocity", -0.5); + p.forward_parking_lane_departure_margin = dp("forward_parking_lane_departure_margin", 0.0); + p.backward_parking_lane_departure_margin = dp("backward_parking_lane_departure_margin", 0.0); p.arc_path_interval = dp("arc_path_interval", 1.0); + p.pull_over_max_steer_angle = dp("pull_over_max_steer_angle", 0.35); // 20deg // hazard p.hazard_on_threshold_dis = dp("hazard_on_threshold_dis", 1.0); p.hazard_on_threshold_vel = dp("hazard_on_threshold_vel", 0.5); @@ -437,29 +447,34 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() PullOutParameters p; - p.min_stop_distance = dp("min_stop_distance", 5.0); - p.stop_time = dp("stop_time", 2.0); - p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0); - p.pull_out_prepare_duration = dp("pull_out_prepare_duration", 2.0); - p.pull_out_duration = dp("pull_out_duration", 4.0); + p.th_arrived_distance = dp("th_arrived_distance", 1.0); + p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); + p.th_stopped_time = dp("th_stopped_time", 1.0); + p.collision_check_margin = dp("collision_check_margin", 1.0); p.pull_out_finish_judge_buffer = dp("pull_out_finish_judge_buffer", 1.0); - p.minimum_pull_out_velocity = dp("minimum_pull_out_velocity", 8.3); - p.prediction_duration = dp("prediction_duration", 8.0); - p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); - p.static_obstacle_velocity_thresh = dp("static_obstacle_velocity_thresh", 0.1); - p.maximum_deceleration = dp("maximum_deceleration", 1.0); + // shift pull out + p.enable_shift_pull_out = dp("enable_shift_pull_out", true); + p.shift_pull_out_velocity = dp("shift_pull_out_velocity", 8.3); p.pull_out_sampling_num = dp("pull_out_sampling_num", 4); - p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); - p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); - p.use_all_predicted_path = dp("use_all_predicted_path", false); - p.use_dynamic_object = dp("use_dynamic_object", false); - p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false); - p.pull_out_search_distance = dp("pull_out_search_distance", 30.0); - p.after_pull_out_straight_distance = dp("after_pull_out_straight_distance", 3.0); p.before_pull_out_straight_distance = dp("before_pull_out_straight_distance", 3.0); + p.minimum_shift_pull_out_distance = dp("minimum_shift_pull_out_distance", 20.0); p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); p.deceleration_interval = dp("deceleration_interval", 10.0); + // geometric pull out + p.enable_geometric_pull_out = dp("enable_geometric_pull_out", true); + p.geometric_pull_out_velocity = dp("geometric_pull_out_velocity", 1.0); + p.arc_path_interval = dp("arc_path_interval", 1.0); + p.lane_departure_margin = dp("lane_departure_margin", 0.2); + p.backward_velocity = dp("backward_velocity", -0.3); + p.pull_out_max_steer_angle = dp("pull_out_max_steer_angle", 0.26); // 15deg + // search start pose backward + p.search_priority = + dp("search_priority", "efficient_path"); // "efficient_path" or "short_back_distance" + p.enable_back = dp("enable_back", true); + p.max_back_distance = dp("max_back_distance", 15.0); + p.backward_search_resolution = dp("backward_search_resolution", 2.0); + p.backward_path_update_duration = dp("backward_path_update_duration", 3.0); // validation of parameters if (p.pull_out_sampling_num < 1) { @@ -469,13 +484,6 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() << "Terminating the program..."); exit(EXIT_FAILURE); } - if (p.maximum_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } return p; } @@ -550,19 +558,22 @@ void BehaviorPathPlannerNode::run() // path handling const auto path = getPath(output, planner_data); - const auto path_candidate = getPathCandidate(output, planner_data); // update planner data planner_data_->prev_output_path = path; mutex_pd_.unlock(); PathWithLaneId clipped_path; - if (skipSmoothGoalConnection(bt_manager_->getModulesStatus())) { + const auto module_status_ptr_vec = bt_manager_->getModulesStatus(); + if (skipSmoothGoalConnection(module_status_ptr_vec)) { clipped_path = *path; } else { clipped_path = modifyPathForSmoothGoalConnection(*path); } - clipPathLength(clipped_path); + + const size_t target_idx = findEgoIndex(clipped_path.points); + util::clipPathLength(clipped_path, target_idx, planner_data_->parameters); + if (!clipped_path.points.empty()) { path_publisher_->publish(clipped_path); } else { @@ -570,6 +581,7 @@ void BehaviorPathPlannerNode::run() get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); } + const auto path_candidate = getPathCandidate(output, planner_data); path_candidate_publisher_->publish(util::toPath(*path_candidate)); // for turn signal @@ -580,8 +592,9 @@ void BehaviorPathPlannerNode::run() turn_signal.command = TurnIndicatorsCommand::DISABLE; hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { + const size_t ego_seg_idx = findEgoSegmentIndex(path->points); turn_signal = turn_signal_decider_.getTurnSignal( - *path, planner_data->self_pose->pose, *(planner_data->route_handler), + *path, planner_data->self_pose->pose, ego_seg_idx, *(planner_data->route_handler), output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance); hazard_signal.command = HazardLightsCommand::DISABLE; } @@ -625,6 +638,13 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( { auto path_candidate = bt_output.path_candidate ? bt_output.path_candidate : std::make_shared(); + + if (isForcedCandidatePath()) { + for (auto & path_point : path_candidate->points) { + path_point.point.longitudinal_velocity_mps = 1.0; + } + } + path_candidate->header = planner_data->route_handler->getRouteHeader(); path_candidate->header.stamp = this->now(); RCLCPP_DEBUG( @@ -633,6 +653,26 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( return path_candidate; } +bool BehaviorPathPlannerNode::isForcedCandidatePath() const +{ + const auto & module_status_ptr_vec = bt_manager_->getModulesStatus(); + for (const auto & module_status_ptr : module_status_ptr_vec) { + if (!module_status_ptr) { + continue; + } + if (module_status_ptr->module_name != "LaneChange") { + continue; + } + const auto & is_waiting_approval = module_status_ptr->is_waiting_approval; + const auto & is_execution_ready = module_status_ptr->is_execution_ready; + if (is_waiting_approval && !is_execution_ready) { + return true; + } + break; + } + return false; +} + bool BehaviorPathPlannerNode::skipSmoothGoalConnection( const std::vector> & statuses) const { @@ -703,13 +743,31 @@ void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) } } -void BehaviorPathPlannerNode::clipPathLength(PathWithLaneId & path) const +SetParametersResult BehaviorPathPlannerNode::onSetParam( + const std::vector & parameters) { - const auto ego_pose = planner_data_->self_pose->pose; - const double forward = planner_data_->parameters.forward_path_length; - const double backward = planner_data_->parameters.backward_path_length; + rcl_interfaces::msg::SetParametersResult result; + + if (!lane_change_param_ptr && !avoidance_param_ptr) { + result.successful = false; + result.reason = "param not initialized"; + return result; + } + + result.successful = true; + result.reason = "success"; + + try { + update_param( + parameters, "avoidance.publish_debug_marker", avoidance_param_ptr->publish_debug_marker); + update_param( + parameters, "lane_change.publish_debug_marker", lane_change_param_ptr->publish_debug_marker); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } - util::clipPathLength(path, ego_pose, forward, backward); + return result; } PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 8ad46b44efb46..4ba819e3e9d56 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -52,34 +52,6 @@ std::vector calcPathArcLengthArray( return out; } -/** - * @brief calc path arclength from start point to end point. - */ -double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) -{ - if (path.points.size() < 2) { - return 0.0; - } - - // swap - bool is_negative_direction = false; - if (start > end) { - std::swap(start, end); - is_negative_direction = true; - } - - start = std::max(start, size_t{1}); - end = std::min(end, path.points.size()); - - double sum = 0.0; - for (size_t i = start; i < end; ++i) { - sum += - tier4_autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); - } - - return is_negative_direction ? -sum : sum; -} - /** * @brief resamplePathWithSpline */ @@ -159,24 +131,17 @@ Path toPath(const PathWithLaneId & input) return output; } -size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const double signed_arc) +size_t getIdxByArclength( + const PathWithLaneId & path, const size_t target_idx, const double signed_arc) { if (path.points.empty()) { throw std::runtime_error("[getIdxByArclength] path points must be > 0"); } - const auto boost_closest_idx = motion_utils::findNearestIndex( - path.points, origin, std::numeric_limits::max(), M_PI / 4.0); - - // If the nearest index search with angle limit fails, search again without angle limit. - size_t closest_idx = boost_closest_idx - ? *boost_closest_idx - : motion_utils::findNearestIndex(path.points, origin.position); - using tier4_autoware_utils::calcDistance2d; double sum_length = 0.0; if (signed_arc >= 0.0) { - for (size_t i = closest_idx; i < path.points.size() - 1; ++i) { + for (size_t i = target_idx; i < path.points.size() - 1; ++i) { const auto next_i = i + 1; sum_length += calcDistance2d(path.points.at(i), path.points.at(next_i)); if (sum_length > signed_arc) { @@ -185,7 +150,7 @@ size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const } return path.points.size() - 1; } else { - for (size_t i = closest_idx; i > 0; --i) { + for (size_t i = target_idx; i > 0; --i) { const auto next_i = i - 1; sum_length -= calcDistance2d(path.points.at(i), path.points.at(next_i)); if (sum_length < signed_arc) { @@ -197,14 +162,14 @@ size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const } void clipPathLength( - PathWithLaneId & path, const Pose base_pose, const double forward, const double backward) + PathWithLaneId & path, const size_t target_idx, const double forward, const double backward) { if (path.points.size() < 3) { return; } - const auto start_idx = util::getIdxByArclength(path, base_pose, -backward); - const auto end_idx = util::getIdxByArclength(path, base_pose, forward); + const auto start_idx = util::getIdxByArclength(path, target_idx, -backward); + const auto end_idx = util::getIdxByArclength(path, target_idx, forward); const std::vector clipped_points{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; @@ -212,6 +177,12 @@ void clipPathLength( path.points = clipped_points; } +void clipPathLength( + PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params) +{ + clipPathLength(path, target_idx, params.forward_path_length, params.backward_path_length); +} + std::pair getPathTurnSignal( const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path, const ShiftPoint & shift_point, const Pose & pose, const double & velocity, 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 6b6c8789e0e46..41f9b43c79e1a 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 @@ -35,7 +35,7 @@ // set as macro so that calling function name will be printed. // debug print is heavy. turn on only when debugging. #define DEBUG_PRINT(...) \ - RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_.print_debug_info, __VA_ARGS__) + RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__) #define printShiftPoints(p, msg) DEBUG_PRINT("[%s] %s", msg, toStrInfo(p).c_str()) namespace behavior_path_planner @@ -47,9 +47,9 @@ using tier4_autoware_utils::calcLateralDeviation; using tier4_planning_msgs::msg::AvoidanceDebugFactor; AvoidanceModule::AvoidanceModule( - const std::string & name, rclcpp::Node & node, const AvoidanceParameters & parameters) + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) : SceneModuleInterface{name, node}, - parameters_{parameters}, + parameters_{std::move(parameters)}, rtc_interface_left_(&node, "avoidance_left"), rtc_interface_right_(&node, "avoidance_right"), uuid_left_{generateUUID()}, @@ -134,7 +134,7 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // reference path data.reference_path = - util::resamplePathWithSpline(center_path, parameters_.resample_interval_for_planning); + util::resamplePathWithSpline(center_path, 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; @@ -173,16 +173,16 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( // velocity filter: only for stopped vehicle const auto objects_candidate = util::filterObjectsByVelocity( - *planner_data_->dynamic_object, parameters_.threshold_speed_object_is_stopped); + *planner_data_->dynamic_object, parameters_->threshold_speed_object_is_stopped); // detection area filter // when expanding lanelets, right_offset must be minus. // This is because y axis is positive on the left. const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( - current_lanes, parameters_.detection_area_left_expand_dist, - parameters_.detection_area_right_expand_dist * (-1.0)); + current_lanes, parameters_->detection_area_left_expand_dist, + parameters_->detection_area_right_expand_dist * (-1.0)); const auto lane_filtered_objects_index = - util::filterObjectsByLanelets(objects_candidate, expanded_lanelets); + util::filterObjectIndicesByLanelets(objects_candidate, expanded_lanelets); DEBUG_PRINT("dynamic_objects size = %lu", planner_data_->dynamic_object->objects.size()); DEBUG_PRINT("object_candidate size = %lu", objects_candidate.objects.size()); @@ -224,11 +224,11 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( avoidance_debug_msg.longitudinal_distance = object_data.longitudinal; // object is behind ego or too far. - if (object_data.longitudinal < -parameters_.object_check_backward_distance) { + if (object_data.longitudinal < -parameters_->object_check_backward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); continue; } - if (object_data.longitudinal > parameters_.object_check_forward_distance) { + if (object_data.longitudinal > parameters_->object_check_forward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); continue; } @@ -260,13 +260,13 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( object_data.overhang_pose.position.x, object_data.overhang_pose.position.y, object_data.overhang_pose.position.z); const bool get_left = - isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction; + isOnRight(object_data) && parameters_->enable_avoidance_over_same_direction; const bool get_right = - !isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction; + !isOnRight(object_data) && parameters_->enable_avoidance_over_same_direction; const auto target_lines = rh->getFurthestLinestring( overhang_lanelet, get_right, get_left, - parameters_.enable_avoidance_over_opposite_direction); + parameters_->enable_avoidance_over_opposite_direction); if (isOnRight(object_data)) { object_data.to_road_shoulder_distance = @@ -287,7 +287,7 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( // Object is on center line -> ignore. avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; - if (std::abs(object_data.lateral) < parameters_.threshold_distance_object_is_on_center) { + if (std::abs(object_data.lateral) < parameters_->threshold_distance_object_is_on_center) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); continue; } @@ -472,10 +472,10 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = getCurrentShift(); // // implement lane detection here. - const auto & lat_collision_safety_buffer = parameters_.lateral_collision_safety_buffer; - const auto & lat_collision_margin = parameters_.lateral_collision_margin; + const auto & lat_collision_safety_buffer = parameters_->lateral_collision_safety_buffer; + const auto & lat_collision_margin = parameters_->lateral_collision_margin; const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto & road_shoulder_safety_margin = parameters_.road_shoulder_safety_margin; + const auto & road_shoulder_safety_margin = parameters_->road_shoulder_safety_margin; const auto avoid_margin = lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width; @@ -544,8 +544,8 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( const auto required_jerk = path_shifter_.calcJerkFromLatLonDistance( avoiding_shift, remaining_distance, getSharpAvoidanceEgoSpeed()); avoidance_debug_msg.required_jerk = required_jerk; - avoidance_debug_msg.maximum_jerk = parameters_.max_lateral_jerk; - if (required_jerk > parameters_.max_lateral_jerk) { + avoidance_debug_msg.maximum_jerk = parameters_->max_lateral_jerk; + if (required_jerk > parameters_->max_lateral_jerk) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_LARGE_JERK); continue; } @@ -556,7 +556,7 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( DEBUG_PRINT( "nominal_lateral_jerk = %f, getNominalAvoidanceEgoSpeed() = %f, prepare_distance = %f, " "has_enough_distance = %d", - parameters_.nominal_lateral_jerk, getNominalAvoidanceEgoSpeed(), prepare_distance, + parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed(), prepare_distance, has_enough_distance); AvoidPoint ap_avoid; @@ -1451,7 +1451,7 @@ void AvoidanceModule::trimTooSharpShift(AvoidPointArray & avoid_points) const const auto isInJerkLimit = [this](const auto & ap) { const auto required_jerk = path_shifter_.calcJerkFromLatLonDistance( ap.getRelativeLength(), ap.getRelativeLongitudinal(), getSharpAvoidanceEgoSpeed()); - return std::fabs(required_jerk) < parameters_.max_lateral_jerk; + return std::fabs(required_jerk) < parameters_->max_lateral_jerk; }; for (size_t i = 0; i < avoid_points_orig.size(); ++i) { @@ -1685,13 +1685,13 @@ void AvoidanceModule::addReturnShiftPointFromEgo( double AvoidanceModule::getRightShiftBound() const { // TODO(Horibe) write me. Real lane boundary must be considered here. - return -parameters_.max_right_shift_length; + return -parameters_->max_right_shift_length; } double AvoidanceModule::getLeftShiftBound() const { // TODO(Horibe) write me. Real lane boundary must be considered here. - return parameters_.max_left_shift_length; + return parameters_->max_left_shift_length; } // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep @@ -1706,13 +1706,13 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c lanelet::ConstLanelets extended_lanelets = current_lanes; for (const auto & current_lane : current_lanes) { - if (!parameters_.enable_avoidance_over_opposite_direction) { + if (!parameters_->enable_avoidance_over_opposite_direction) { break; } const auto extend_from_current_lane = std::invoke( [this, &route_handler](const lanelet::ConstLanelet & lane) { - const auto ignore_opposite = !parameters_.enable_avoidance_over_opposite_direction; + const auto ignore_opposite = !parameters_->enable_avoidance_over_opposite_direction; if (ignore_opposite) { return route_handler->getAllSharedLineStringLanelets(lane, true, true, ignore_opposite); } @@ -1825,8 +1825,8 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted if (t < NO_ACCEL_TIME_THR) { insert_idx = i; vmax = std::max( - parameters_.min_avoidance_speed_for_acc_prevention, - std::sqrt(v0 * v0 + 2.0 * s * parameters_.max_avoidance_acceleration)); + parameters_->min_avoidance_speed_for_acc_prevention, + std::sqrt(v0 * v0 + 2.0 * s * parameters_->max_avoidance_acceleration)); break; } } @@ -1840,7 +1840,7 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted DEBUG_PRINT( "s: %f, t: %f, v0: %f, a: %f, vmax: %f, ego_i: %lu, target_i: %lu", s_from_ego, t_from_ego, v0, - parameters_.max_avoidance_acceleration, vmax, ego_idx, target_idx); + parameters_->max_avoidance_acceleration, vmax, ego_idx, target_idx); } // TODO(Horibe) clean up functions: there is a similar code in util as well. @@ -2029,8 +2029,10 @@ BehaviorModuleOutput AvoidanceModule::plan() prev_linear_shift_path_ = toShiftedPath(avoidance_data_.reference_path); path_shifter_.generate(&prev_linear_shift_path_, true, SHIFT_TYPE::LINEAR); prev_reference_ = avoidance_data_.reference_path; - if (parameters_.publish_debug_marker) { + if (parameters_->publish_debug_marker) { setDebugData(path_shifter_, debug_data_); + } else { + debug_marker_.markers.clear(); } } @@ -2039,11 +2041,12 @@ BehaviorModuleOutput AvoidanceModule::plan() // sparse resampling for computational cost { avoidance_path.path = - util::resamplePathWithSpline(avoidance_path.path, parameters_.resample_interval_for_output); + util::resamplePathWithSpline(avoidance_path.path, parameters_->resample_interval_for_output); } output.path = std::make_shared(avoidance_path.path); - clipPathLength(*output.path); + const size_t ego_idx = findEgoIndex(output.path->points); + util::clipPathLength(*output.path, ego_idx, planner_data_->parameters); DEBUG_PRINT("exit plan(): set prev output (back().lat = %f)", prev_output_.shift_length.back()); @@ -2084,7 +2087,8 @@ CandidateOutput AvoidanceModule::planCandidate() const output.distance_to_path_change = new_shift_points->front().start_longitudinal; } - clipPathLength(shifted_path.path); + const size_t ego_idx = findEgoIndex(shifted_path.path.points); + util::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); output.path_candidate = shifted_path.path; @@ -2238,9 +2242,9 @@ boost::optional AvoidanceModule::findNewShiftPoint( // TODO(Horibe) test fails with this print. why? // DEBUG_PRINT("%s, shift current: %f, candidate: %f", pfx, current_shift, candidate.length); - const auto new_point_threshold = parameters_.avoidance_execution_lateral_threshold; + const auto new_point_threshold = parameters_->avoidance_execution_lateral_threshold; if (std::abs(candidate.length - current_shift) > new_point_threshold) { - if (calcJerk(candidate) > parameters_.max_lateral_jerk) { + if (calcJerk(candidate) > parameters_->max_lateral_jerk) { DEBUG_PRINT( "%s, Failed to find new shift: jerk limit over (%f).", pfx, calcJerk(candidate)); break; @@ -2264,11 +2268,11 @@ double AvoidanceModule::getEgoSpeed() const double AvoidanceModule::getNominalAvoidanceEgoSpeed() const { - return std::max(getEgoSpeed(), parameters_.min_nominal_avoidance_speed); + return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); } double AvoidanceModule::getSharpAvoidanceEgoSpeed() const { - return std::max(getEgoSpeed(), parameters_.min_sharp_avoidance_speed); + return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); } Point AvoidanceModule::getEgoPosition() const { return planner_data_->self_pose->pose.position; } @@ -2299,25 +2303,25 @@ double AvoidanceModule::getNominalAvoidanceDistance(const double shift_length) c { const auto & p = parameters_; const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_.nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); + shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); - return std::max(p.min_avoidance_distance, distance_by_jerk); + return std::max(p->min_avoidance_distance, distance_by_jerk); } double AvoidanceModule::getSharpAvoidanceDistance(const double shift_length) const { const auto & p = parameters_; const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_.max_lateral_jerk, getSharpAvoidanceEgoSpeed()); + shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); - return std::max(p.min_avoidance_distance, distance_by_jerk); + return std::max(p->min_avoidance_distance, distance_by_jerk); } double AvoidanceModule::getNominalPrepareDistance() const { const auto & p = parameters_; const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. - const auto nominal_distance = std::max(getEgoSpeed() * p.prepare_time, p.min_prepare_distance); + const auto nominal_distance = std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); return nominal_distance + epsilon_m; } @@ -2337,7 +2341,8 @@ ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) c void AvoidanceModule::postProcess(PathShifter & path_shifter) const { - path_shifter.removeBehindShiftPointAndSetBaseOffset(getEgoPosition()); + const size_t nearest_idx = findEgoIndex(path_shifter.getReferencePath().points); + path_shifter.removeBehindShiftPointAndSetBaseOffset(getEgoPose().pose, nearest_idx); } void AvoidanceModule::updateData() @@ -2422,7 +2427,7 @@ void AvoidanceModule::updateRegisteredObject(const ObjectDataArray & now_objects } // lost count exceeds threshold. remove object from register. - if (r.lost_time > parameters_.object_last_seen_threshold) { + if (r.lost_time > parameters_->object_last_seen_threshold) { registered_objects_.erase(registered_objects_.begin() + i); } } @@ -2481,11 +2486,6 @@ void AvoidanceModule::onExit() removeRTCStatus(); } -void AvoidanceModule::setParameters(const AvoidanceParameters & parameters) -{ - parameters_ = parameters; -} - void AvoidanceModule::initVariables() { prev_output_ = ShiftedPath(); @@ -2503,27 +2503,19 @@ void AvoidanceModule::initVariables() original_unique_id = 0; } -void AvoidanceModule::clipPathLength(PathWithLaneId & path) const -{ - const double forward = planner_data_->parameters.forward_path_length; - const double backward = planner_data_->parameters.backward_path_length; - - util::clipPathLength(path, getEgoPose().pose, forward, backward); -} - bool AvoidanceModule::isTargetObjectType(const PredictedObject & object) const { using autoware_auto_perception_msgs::msg::ObjectClassification; const auto t = util::getHighestProbLabel(object.classification); const auto is_object_type = - ((t == ObjectClassification::CAR && parameters_.avoid_car) || - (t == ObjectClassification::TRUCK && parameters_.avoid_truck) || - (t == ObjectClassification::BUS && parameters_.avoid_bus) || - (t == ObjectClassification::TRAILER && parameters_.avoid_trailer) || - (t == ObjectClassification::UNKNOWN && parameters_.avoid_unknown) || - (t == ObjectClassification::BICYCLE && parameters_.avoid_bicycle) || - (t == ObjectClassification::MOTORCYCLE && parameters_.avoid_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && parameters_.avoid_pedestrian)); + ((t == ObjectClassification::CAR && parameters_->avoid_car) || + (t == ObjectClassification::TRUCK && parameters_->avoid_truck) || + (t == ObjectClassification::BUS && parameters_->avoid_bus) || + (t == ObjectClassification::TRAILER && parameters_->avoid_trailer) || + (t == ObjectClassification::UNKNOWN && parameters_->avoid_unknown) || + (t == ObjectClassification::BICYCLE && parameters_->avoid_bicycle) || + (t == ObjectClassification::MOTORCYCLE && parameters_->avoid_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && parameters_->avoid_pedestrian)); return is_object_type; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp new file mode 100644 index 0000000000000..908087baeeef9 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/lane_change/debug.cpp @@ -0,0 +1,237 @@ +// 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 "behavior_path_planner/scene_module/utils/path_shifter.hpp" + +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace marker_utils::lane_change_markers +{ +using geometry_msgs::msg::Point; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +MarkerArray showObjectInfo( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + + MarkerArray marker_array; + int32_t id{0}; + + marker_array.markers.reserve(obj_debug_vec.size()); + + for (const auto & [uuid, info] : obj_debug_vec) { + marker.id = ++id; + marker.pose = info.current_pose; + + std::ostringstream ss; + + ss << info.failed_reason << "\nLon: " << std::setprecision(4) << info.relative_to_ego.position.x + << "\nLat: " << info.relative_to_ego.position.y; + + marker.text = ss.str(); + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) +{ + if (lanes.empty()) { + return MarkerArray{}; + } + + MarkerArray marker_array; + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + + constexpr auto colors = colorsList(); + const auto loop_size = std::min(lanes.size(), colors.size()); + marker_array.markers.reserve(loop_size); + + for (std::size_t idx = 0; idx < loop_size; ++idx) { + if (lanes.at(idx).path.points.empty()) { + continue; + } + + const auto & color = colors.at(idx); + Marker marker = createDefaultMarker( + "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), + createMarkerColor(color[0], color[1], color[2], 0.9)); + marker.points.reserve(lanes.at(idx).path.points.size()); + + for (const auto & point : lanes.at(idx).path.points) { + marker.points.push_back(point.point.pose.position); + } + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray showLerpedPose( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + MarkerArray marker_array; + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + marker_array.markers.reserve(obj_debug_vec.size()); + + for (const auto & [uuid, info] : obj_debug_vec) { + Marker marker = createDefaultMarker( + "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), + createMarkerColor(1.0, 0.3, 1.0, 0.9)); + marker.points.reserve(info.lerped_path.size()); + + for (const auto & point : info.lerped_path) { + marker.points.push_back(point.position); + } + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +MarkerArray showEgoPredictedPaths( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + MarkerArray marker_array; + constexpr auto colors = colorsList(); + constexpr float scale_val = 0.2; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + marker_array.markers.reserve(obj_debug_vec.size()); + + int32_t id{0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto loop_size = std::min(info.ego_predicted_path.size(), colors.size()); + + for (std::size_t idx = 0; idx < loop_size; ++idx) { + const auto & path = info.ego_predicted_path.at(idx).path; + const auto & color = colors.at(idx); + + Marker marker = createDefaultMarker( + "map", current_time, ns, ++id, Marker::LINE_STRIP, + createMarkerScale(scale_val, scale_val, scale_val), + createMarkerColor(color[0], color[1], color[2], 0.9)); + + marker.points.reserve(path.size()); + + for (const auto & point : path) { + marker.points.push_back(point.position); + } + + marker_array.markers.push_back(marker); + } + } + return marker_array; +} + +MarkerArray showPolygon( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + constexpr float scale_val{0.2}; + int32_t id{0}; + Marker ego_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(scale_val, scale_val, scale_val), createMarkerColor(1.0, 1.0, 1.0, 0.9)); + Marker obj_marker = ego_marker; + MarkerArray marker_array; + + constexpr auto colors = colorsList(); + const auto loop_size = std::min(obj_debug_vec.size(), colors.size()); + + marker_array.markers.reserve(loop_size * 2); + size_t idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + if (idx >= loop_size) { + break; + } + const auto & color = colors.at(idx); + const auto & ego_polygon = info.ego_polygon.outer(); + ego_marker.id = ++id; + ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); + ego_marker.points.reserve(ego_polygon.size()); + for (const auto & p : ego_polygon) { + ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + } + marker_array.markers.push_back(ego_marker); + + const auto & obj_polygon = info.obj_polygon.outer(); + obj_marker.id = ++id; + obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); + obj_marker.points.reserve(obj_polygon.size()); + for (const auto & p : obj_polygon) { + obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + } + marker_array.markers.push_back(obj_marker); + ++idx; + ego_marker.points.clear(); + obj_marker.points.clear(); + } + + return marker_array; +} + +MarkerArray showPolygonPose( + const std::unordered_map & obj_debug_vec, std::string && ns) +{ + constexpr auto colors = colorsList(); + const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); + MarkerArray marker_array; + int32_t id{0}; + size_t idx{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + marker_array.markers.reserve(obj_debug_vec.size()); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto & color = colors.at(idx); + Marker marker = createDefaultMarker( + "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(color[0], color[1], color[2], 0.999)); + marker.points.reserve(2); + marker.points.push_back(info.expected_ego_pose.position); + marker.points.push_back(info.expected_obj_pose.position); + marker_array.markers.push_back(marker); + ++idx; + if (idx >= loop_size) { + break; + } + } + + return marker_array; +} +} // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 47c69274564ea..f43a683ba610d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/lane_change/util.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utilities.hpp" #include @@ -35,12 +36,11 @@ namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; -using route_handler::LaneChangeDirection; LaneChangeModule::LaneChangeModule( - const std::string & name, rclcpp::Node & node, const LaneChangeParameters & parameters) + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) : SceneModuleInterface{name, node}, - parameters_{parameters}, + parameters_{std::move(parameters)}, rtc_interface_left_(&node, "lane_change_left"), rtc_interface_right_(&node, "lane_change_right"), uuid_left_{generateUUID()}, @@ -79,6 +79,7 @@ void LaneChangeModule::onExit() { clearWaitingApproval(); removeRTCStatus(); + debug_marker_.markers.clear(); current_state_ = BT::NodeStatus::IDLE; RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit"); } @@ -94,9 +95,8 @@ bool LaneChangeModule::isExecutionRequested() const const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); return found_valid_path; @@ -112,13 +112,11 @@ bool LaneChangeModule::isExecutionReady() const const auto current_lanes = getCurrentLanes(); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); - return found_safe_path && !isLaneBlocked(lane_change_lanes); + return found_safe_path; } BT::NodeStatus LaneChangeModule::updateState() @@ -182,13 +180,15 @@ CandidateOutput LaneChangeModule::planCandidate() const const auto current_lanes = getCurrentLanes(); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + [[maybe_unused]] const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); + if (selected_path.path.points.empty()) { + return output; + } + const auto start_idx = selected_path.shift_point.start_idx; const auto end_idx = selected_path.shift_point.end_idx; @@ -213,11 +213,6 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() return out; } -void LaneChangeModule::setParameters(const LaneChangeParameters & parameters) -{ - parameters_ = parameters; -} - void LaneChangeModule::updateLaneChangeStatus() { const auto current_lanes = getCurrentLanes(); @@ -228,9 +223,8 @@ void LaneChangeModule::updateLaneChangeStatus() status_.lane_change_lanes = lane_change_lanes; // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); // Update status @@ -286,7 +280,7 @@ PathWithLaneId LaneChangeModule::getReferencePath() const num_lane_change * (common_parameters.minimum_lane_change_length + buffer); reference_path = util::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration, + *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, lane_change_buffer); reference_path.drivable_area = util::generateDrivableArea( @@ -332,7 +326,7 @@ lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( lanelet::utils::query::getClosestLanelet( current_lanes, planner_data_->self_pose->pose, ¤t_lane); const double lane_change_prepare_length = std::max( - current_twist.linear.x * parameters_.lane_change_prepare_duration, + current_twist.linear.x * parameters_->lane_change_prepare_duration, planner_data_->parameters.minimum_lane_change_length); lanelet::ConstLanelets current_check_lanes = route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); @@ -364,7 +358,7 @@ std::pair LaneChangeModule::getSafePath( // find candidate paths const auto lane_change_paths = lane_change_utils::getLaneChangePaths( *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - common_parameters, parameters_); + common_parameters, *parameters_); // get lanes used for detection lanelet::ConstLanelets check_lanes; @@ -386,11 +380,20 @@ std::pair LaneChangeModule::getSafePath( if (valid_paths.empty()) { return std::make_pair(false, false); } + debug_valid_path_ = valid_paths; // select safe path + object_debug_.clear(); bool found_safe_path = lane_change_utils::selectSafePath( valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, &safe_path); + current_twist, common_parameters, *parameters_, &safe_path, object_debug_); + + if (parameters_->publish_debug_marker) { + setObjectDebugVisualization(); + } else { + debug_marker_.markers.clear(); + } + return std::make_pair(true, found_safe_path); } @@ -416,54 +419,6 @@ bool LaneChangeModule::isCurrentSpeedLow() const return util::l2Norm(current_twist.linear) < threshold_kmph * 1000 / 3600; } -bool LaneChangeModule::isLaneBlocked(const lanelet::ConstLanelets & lanes) const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - - const auto current_lanes = getCurrentLanes(); - - const auto arc = lanelet::utils::getArcCoordinates(lanes, current_pose); - constexpr double max_check_distance = 100; - double static_obj_velocity_thresh = parameters_.static_obstacle_velocity_thresh; - const double lane_changeable_distance_left = - route_handler->getLaneChangeableDistance(current_pose, LaneChangeDirection::LEFT); - const double lane_changeable_distance_right = - route_handler->getLaneChangeableDistance(current_pose, LaneChangeDirection::RIGHT); - const double lane_changeable_distance = - std::max(lane_changeable_distance_left, lane_changeable_distance_right); - const double check_distance = std::min(max_check_distance, lane_changeable_distance); - const auto polygon = - lanelet::utils::getPolygonFromArcLength(lanes, arc.length, arc.length + check_distance); - - if (polygon.size() < 3) { - RCLCPP_WARN_STREAM( - getLogger(), "could not get polygon from lanelet with arc lengths: " - << arc.length << " to " << arc.length + check_distance); - return false; - } - - for (const auto & obj : planner_data_->dynamic_object->objects) { - const auto label = util::getHighestProbLabel(obj.classification); - if ( - label == ObjectClassification::CAR || label == ObjectClassification::TRUCK || - label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE) { - const auto velocity = util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); - if (velocity < static_obj_velocity_thresh) { - const auto position = lanelet::utils::conversion::toLaneletPoint( - obj.kinematics.initial_pose_with_covariance.pose.position); - const auto distance = boost::geometry::distance( - lanelet::utils::to2D(position).basicPoint(), - lanelet::utils::to2D(polygon).basicPolygon()); - if (distance < std::numeric_limits::epsilon()) { - return true; - } - } - } - } - return false; -} - bool LaneChangeModule::isAbortConditionSatisfied() const { const auto & route_handler = planner_data_->route_handler; @@ -475,7 +430,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto current_lanes = status_.current_lanes; // check abort enable flag - if (!parameters_.enable_abort_lane_change) { + if (!parameters_->enable_abort_lane_change) { return false; } @@ -504,14 +459,16 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto check_lanes = route_handler->getCheckTargetLanesFromPath( path.path, status_.lane_change_lanes, check_distance_with_path); + const size_t current_seg_idx = findEgoSegmentIndex(path.path.points); + std::unordered_map debug_data; is_path_safe = lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, objects, current_pose, current_twist, - common_parameters.vehicle_width, parameters_, false, status_.lane_change_path.acceleration); + path.path, current_lanes, check_lanes, objects, current_pose, current_seg_idx, current_twist, + common_parameters, *parameters_, debug_data, false, status_.lane_change_path.acceleration); } // check vehicle velocity thresh const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_.abort_lane_change_velocity_thresh; + util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; // check if vehicle is within lane bool is_within_original_lane = false; @@ -531,7 +488,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto centerline2d = lanelet::utils::to2D(closest_lanelet.centerline()).basicLineString(); lanelet::BasicPoint2d vehicle_pose2d(current_pose.position.x, current_pose.position.y); const double distance = lanelet::geometry::distance2d(centerline2d, vehicle_pose2d); - is_distance_small = distance < parameters_.abort_lane_change_distance_thresh; + is_distance_small = distance < parameters_->abort_lane_change_distance_thresh; } // check angle thresh from original lane @@ -541,7 +498,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); const double vehicle_yaw = tf2::getYaw(current_pose.orientation); const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); - is_angle_diff_small = std::abs(yaw_diff) < parameters_.abort_lane_change_angle_thresh; + is_angle_diff_small = std::abs(yaw_diff) < parameters_->abort_lane_change_angle_thresh; } // abort only if velocity is low or vehicle pose is close enough @@ -569,8 +526,28 @@ bool LaneChangeModule::hasFinishedLaneChange() const const double travel_distance = arclength_current.length - status_.start_distance; const double finish_distance = status_.lane_change_path.preparation_length + status_.lane_change_path.lane_change_length + - parameters_.lane_change_finish_judge_buffer; + parameters_->lane_change_finish_judge_buffer; return travel_distance > finish_distance; } +void LaneChangeModule::setObjectDebugVisualization() const +{ + using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showLerpedPose; + using marker_utils::lane_change_markers::showObjectInfo; + using marker_utils::lane_change_markers::showPolygon; + using marker_utils::lane_change_markers::showPolygonPose; + + debug_marker_.markers.clear(); + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; + + add(showObjectInfo(object_debug_, "object_debug_info")); + add(showLerpedPose(object_debug_, "lerp_pose_before_true")); + add(showPolygonPose(object_debug_, "expected_pose")); + add(showPolygon(object_debug_, "lerped_polygon")); + add(showAllValidLaneChangePath(debug_valid_path_, "lane_change_valid_paths")); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 250390eafdd67..7a8a4d27cd19f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/util.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" +#include "behavior_path_planner/utilities.hpp" #include #include @@ -30,9 +31,7 @@ #include #include -namespace behavior_path_planner -{ -namespace lane_change_utils +namespace behavior_path_planner::lane_change_utils { PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) { @@ -67,7 +66,19 @@ bool isPathInLanelets( } return true; } +double getExpectedVelocityWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration) +{ + return velocity + expected_acceleration * duration; +} +double getDistanceWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration, + const double & minimum_distance) +{ + const auto distance = velocity * duration + 0.5 * expected_acceleration * std::pow(duration, 2); + return std::max(distance, minimum_distance); +} std::vector getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, @@ -80,17 +91,19 @@ std::vector getLaneChangePaths( } // rename parameter - const double backward_path_length = common_parameter.backward_path_length; - const double forward_path_length = common_parameter.forward_path_length; - const double lane_change_prepare_duration = parameter.lane_change_prepare_duration; - const double lane_changing_duration = parameter.lane_changing_duration; - const double minimum_lane_change_length = common_parameter.minimum_lane_change_length; - const double minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; - const double maximum_deceleration = parameter.maximum_deceleration; - const int lane_change_sampling_num = parameter.lane_change_sampling_num; + const auto & backward_path_length = common_parameter.backward_path_length; + const auto & forward_path_length = common_parameter.forward_path_length; + const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration; + const auto & lane_changing_duration = parameter.lane_changing_duration; + const auto & minimum_lane_change_prepare_distance = + parameter.minimum_lane_change_prepare_distance; + const auto & minimum_lane_change_length = common_parameter.minimum_lane_change_length; + const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; + const auto & maximum_deceleration = parameter.maximum_deceleration; + const auto & lane_change_sampling_num = parameter.lane_change_sampling_num; // get velocity - const double v0 = util::l2Norm(twist.linear); + const double current_velocity = util::l2Norm(twist.linear); constexpr double buffer = 1.0; // buffer for min_lane_change_length const double acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; @@ -100,23 +113,25 @@ std::vector getLaneChangePaths( for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { - PathWithLaneId reference_path{}; - const double v1 = v0 + acceleration * lane_change_prepare_duration; - const double v2 = v1 + acceleration * lane_changing_duration; + const double lane_change_prepare_velocity = getExpectedVelocityWhenDecelerate( + current_velocity, acceleration, lane_change_prepare_duration); + const double lane_changing_velocity = getExpectedVelocityWhenDecelerate( + lane_change_prepare_velocity, acceleration, lane_changing_duration); // skip if velocity becomes less than zero before finishing lane change - if (v2 < 0.0) { + if (lane_changing_velocity < 0.0) { continue; } // get path on original lanes - const double straight_distance = v0 * lane_change_prepare_duration + - 0.5 * acceleration * std::pow(lane_change_prepare_duration, 2); - double lane_change_distance = - v1 * lane_changing_duration + 0.5 * acceleration * std::pow(lane_changing_duration, 2); - lane_change_distance = std::max(lane_change_distance, minimum_lane_change_length); - - if (straight_distance < target_distance) { + const double prepare_distance = getDistanceWhenDecelerate( + current_velocity, acceleration, lane_change_prepare_duration, + minimum_lane_change_prepare_distance); + const double lane_changing_distance = getDistanceWhenDecelerate( + lane_change_prepare_velocity, acceleration, lane_changing_duration, + minimum_lane_change_length + buffer); + + if (prepare_distance < target_distance) { continue; } @@ -124,21 +139,21 @@ std::vector getLaneChangePaths( { const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, pose); const double s_start = arc_position.length - backward_path_length; - const double s_end = arc_position.length + straight_distance; + const double s_end = arc_position.length + prepare_distance; reference_path1 = route_handler.getCenterLinePath(original_lanelets, s_start, s_end); } reference_path1.points.back().point.longitudinal_velocity_mps = std::min( reference_path1.points.back().point.longitudinal_velocity_mps, static_cast( - std::max(straight_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); + std::max(prepare_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); PathWithLaneId reference_path2{}; { const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); const auto arc_position = lanelet::utils::getArcCoordinates(target_lanelets, pose); const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); - double s_start = arc_position.length + straight_distance + lane_change_distance; + double s_start = arc_position.length + prepare_distance + lane_changing_distance; s_start = std::min(s_start, lane_length - num * minimum_lane_change_length); double s_end = s_start + forward_path_length; s_end = std::min(s_end, lane_length - num * (minimum_lane_change_length + buffer)); @@ -150,7 +165,7 @@ std::vector getLaneChangePaths( point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + std::max(lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); } if (reference_path1.points.empty() || reference_path2.points.empty()) { @@ -162,8 +177,8 @@ std::vector getLaneChangePaths( LaneChangePath candidate_path; candidate_path.acceleration = acceleration; - candidate_path.preparation_length = straight_distance; - candidate_path.lane_change_length = lane_change_distance; + candidate_path.preparation_length = prepare_distance; + candidate_path.lane_change_length = lane_changing_distance; PathWithLaneId target_lane_reference_path; { @@ -171,7 +186,7 @@ std::vector getLaneChangePaths( lanelet::utils::getArcCoordinates( target_lanelets, reference_path1.points.back().point.pose); double s_start = lane_change_start_arc_position.length; - double s_end = s_start + straight_distance + lane_change_distance + forward_path_length; + double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); } @@ -221,8 +236,8 @@ std::vector getLaneChangePaths( } point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, - static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + static_cast(std::max( + lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); const auto nearest_idx = motion_utils::findNearestIndex(reference_path2.points, point.point.pose); point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids; @@ -272,18 +287,21 @@ bool selectSafePath( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, - const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path) + const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path, + std::unordered_map & debug_data) { for (const auto & path : paths) { + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); if (isLaneChangePathSafe( - path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_twist, - vehicle_width, ros_parameters, true, path.acceleration)) { + path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, + current_twist, common_parameters, ros_parameters, debug_data, true, path.acceleration)) { *selected_path = path; return true; } } - // set first path for force lane change if no valid path found if (!paths.empty()) { *selected_path = paths.front(); @@ -292,7 +310,6 @@ bool selectSafePath( return false; } - bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, @@ -330,92 +347,86 @@ bool isLaneChangePathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, - const LaneChangeParameters & ros_parameters, const bool use_buffer, const double acceleration) + const size_t current_seg_idx, const Twist & current_twist, + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, + std::unordered_map & debug_data, const bool use_buffer, + const double acceleration) { - if (path.points.empty()) { - return false; - } - if (target_lanes.empty() || current_lanes.empty()) { - return false; - } if (dynamic_objects == nullptr) { return true; } - const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); - constexpr double check_distance = 100.0; - // parameters - const double time_resolution = ros_parameters.prediction_time_resolution; - const double min_thresh = ros_parameters.min_stop_distance; - const double stop_time = ros_parameters.stop_time; - - double buffer; - double lateral_buffer; - if (use_buffer) { - buffer = ros_parameters.hysteresis_buffer_distance; - lateral_buffer = 0.5; - } else { - buffer = 0.0; - lateral_buffer = 0.0; - } - double current_lane_check_start_time = 0.0; - const double current_lane_check_end_time = - ros_parameters.lane_change_prepare_duration + ros_parameters.lane_changing_duration; - double target_lane_check_start_time = 0.0; - const double target_lane_check_end_time = - ros_parameters.lane_change_prepare_duration + ros_parameters.lane_changing_duration; - if (!ros_parameters.enable_collision_check_at_prepare_phase) { - current_lane_check_start_time = ros_parameters.lane_change_prepare_duration; - target_lane_check_start_time = ros_parameters.lane_change_prepare_duration; + if (path.points.empty() || target_lanes.empty() || current_lanes.empty()) { + return false; } + const double time_resolution = lane_change_parameters.prediction_time_resolution; + const auto & lane_change_prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const auto & lane_changing_duration = lane_change_parameters.lane_changing_duration; + const double current_lane_check_start_time = + (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration + : 0.0; + const double current_lane_check_end_time = lane_change_prepare_duration + lane_changing_duration; + double target_lane_check_start_time = + (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration + : 0.0; + const double target_lane_check_end_time = lane_change_prepare_duration + lane_changing_duration; + constexpr double ego_predicted_path_min_speed{1.0}; + const auto vehicle_predicted_path = util::convertToPredictedPath( + path, current_twist, current_pose, current_seg_idx, target_lane_check_end_time, time_resolution, + acceleration, ego_predicted_path_min_speed); + + const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + // find obstacle in lane change target lanes // retrieve lanes that are merging target lanes as well const auto target_lane_object_indices = - util::filterObjectsByLanelets(*dynamic_objects, target_lanes); + util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); // find objects in current lane - const auto current_lane_object_indices_lanelet = util::filterObjectsByLanelets( + constexpr double check_distance = 100.0; + const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); - const auto current_lane_object_indices = util::filterObjectsByPath( + + const double lateral_buffer = (use_buffer) ? 0.5 : 0.0; + const auto & vehicle_width = common_parameters.vehicle_width; + const auto & vehicle_length = common_parameters.vehicle_length; + const auto current_lane_object_indices = util::filterObjectsIndicesByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, vehicle_width / 2 + lateral_buffer); - const auto & vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration); + const auto assignDebugData = [](const PredictedObject & obj) { + CollisionCheckDebug debug; + const auto key = util::getUuidStr(obj); + debug.current_pose = obj.kinematics.initial_pose_with_covariance.pose; + debug.current_twist = obj.kinematics.initial_twist_with_covariance.twist; + return std::make_pair(key, debug); + }; + + const auto appendDebugInfo = + [&debug_data](std::pair & obj, bool && is_allowed) { + const auto & key = obj.first; + auto & element = obj.second; + element.allow_lane_change = is_allowed; + if (debug_data.find(key) != debug_data.end()) { + debug_data[key] = element; + } else { + debug_data.insert(obj); + } + }; - // Collision check for objects in current lane for (const auto & i : current_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - if (ros_parameters.use_all_predicted_path) { - predicted_paths.resize(obj.kinematics.predicted_paths.size()); - std::copy( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - predicted_paths.begin()); - } else { - auto & max_confidence_path = *(std::max_element( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - [](const auto & path1, const auto & path2) { - return path1.confidence > path2.confidence; - })); - predicted_paths.push_back(max_confidence_path); - } + auto current_debug_data = assignDebugData(obj); + const auto predicted_paths = + util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); for (const auto & obj_path : predicted_paths) { - double distance = util::getDistanceBetweenPredictedPaths( - obj_path, vehicle_predicted_path, current_lane_check_start_time, - current_lane_check_end_time, time_resolution); - double thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = util::l2Norm(current_twist.linear) * stop_time; - } else { - thresh = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear) * stop_time; - } - thresh = std::max(thresh, min_thresh); - thresh += buffer; - if (distance < thresh) { + if (!util::isSafeInLaneletCollisionCheck( + current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, + current_lane_check_start_time, current_lane_check_end_time, time_resolution, obj, + obj_path, common_parameters, current_debug_data.second)) { + appendDebugInfo(current_debug_data, false); return false; } } @@ -424,23 +435,10 @@ bool isLaneChangePathSafe( // Collision check for objects in lane change target lane for (const auto & i : target_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - if (ros_parameters.use_all_predicted_path) { - predicted_paths.resize(obj.kinematics.predicted_paths.size()); - std::copy( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - predicted_paths.begin()); - } else { - auto & max_confidence_path = *(std::max_element( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - [](const auto & path1, const auto & path2) { - return path1.confidence > path2.confidence; - })); - predicted_paths.push_back(max_confidence_path); - } - + auto current_debug_data = assignDebugData(obj); + current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); bool is_object_in_target = false; - if (ros_parameters.use_predicted_path_outside_lanelet) { + if (lane_change_parameters.use_predicted_path_outside_lanelet) { is_object_in_target = true; } else { for (const auto & llt : target_lanes) { @@ -450,53 +448,30 @@ bool isLaneChangePathSafe( } } + const auto predicted_paths = + util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); + if (is_object_in_target) { for (const auto & obj_path : predicted_paths) { - const double distance = util::getDistanceBetweenPredictedPaths( - obj_path, vehicle_predicted_path, target_lane_check_start_time, - target_lane_check_end_time, time_resolution); - double thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = util::l2Norm(current_twist.linear) * stop_time; - } else { - thresh = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear) * stop_time; - } - thresh = std::max(thresh, min_thresh); - thresh += buffer; - if (distance < thresh) { + if (!util::isSafeInLaneletCollisionCheck( + current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, + target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, + obj_path, common_parameters, current_debug_data.second)) { + appendDebugInfo(current_debug_data, false); return false; } } } else { - const double distance = util::getDistanceBetweenPredictedPathAndObject( - obj, vehicle_predicted_path, target_lane_check_start_time, target_lane_check_end_time, - time_resolution); - double thresh = min_thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = std::max(thresh, util::l2Norm(current_twist.linear) * stop_time); - } - thresh += buffer; - if (distance < thresh) { + if (!util::isSafeInFreeSpaceCollisionCheck( + current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, + target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, + common_parameters, current_debug_data.second)) { + appendDebugInfo(current_debug_data, false); return false; } } + appendDebugInfo(current_debug_data, true); } - return true; } - -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) -{ - tf2::Transform tf_map2ego{}; - tf2::Transform tf_map2obj{}; - Pose obj_from_ego{}; - tf2::fromMsg(ego_pose, tf_map2ego); - tf2::fromMsg(obj_pose, tf_map2obj); - tf2::toMsg(tf_map2ego.inverse() * tf_map2obj, obj_from_ego); - - return obj_from_ego.position.x > 0; -} - -} // namespace lane_change_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index f9679d09b6f52..5215afdbac2d2 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -111,7 +111,9 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const p.forward_path_length, p); // clip backward length - util::clipPathLength(reference_path, current_pose, p.forward_path_length, p.backward_path_length); + const size_t current_seg_idx = findEgoSegmentIndex(reference_path.points); + util::clipPathLength( + reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length); { double optional_lengths{0.0}; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp new file mode 100644 index 0000000000000..eec8731bbda4e --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -0,0 +1,68 @@ +// 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 "behavior_path_planner/scene_module/pull_out/geometric_pull_out.hpp" + +#include "behavior_path_planner/scene_module/pull_out/util.hpp" + +using lanelet::utils::getArcCoordinates; +using motion_utils::findNearestIndex; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +namespace behavior_path_planner +{ +using pull_out_utils::combineReferencePath; +using pull_out_utils::getPullOutLanes; + +GeometricPullOut::GeometricPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + const ParallelParkingParameters & parallel_parking_parameters) +: PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parallel_parking_parameters} +{ +} + +boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +{ + PullOutPath output; + + // conbine road lane and shoulder lane + const auto road_lanes = util::getCurrentLanes(planner_data_); + const auto shoulder_lanes = getPullOutLanes(road_lanes, planner_data_); + auto lanes = road_lanes; + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + // todo: set params only once? + planner_.setData(planner_data_, parallel_parking_parameters_); + const bool found_valid_path = + planner_.planPullOut(start_pose, goal_pose, road_lanes, shoulder_lanes); + if (!found_valid_path) { + return {}; + } + + // collsion check with objects in shoulder lanes + const auto arc_path = planner_.getArcPath(); + const auto shoulder_lane_objects = + util::filterObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) { + return {}; + } + + output.partial_paths = planner_.getPaths(); + output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose; + output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; + + return output; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 362963fc87d6c..450ba7f5e31db 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -29,13 +29,32 @@ #include #include +using motion_utils::calcLongitudinalOffsetPose; +using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { PullOutModule::PullOutModule( const std::string & name, rclcpp::Node & node, const PullOutParameters & parameters) -: SceneModuleInterface{name, node}, parameters_{parameters} +: SceneModuleInterface{name, node}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { rtc_interface_ptr_ = std::make_shared(&node, "pull_out"); + lane_departure_checker_ = std::make_shared(); + lane_departure_checker_->setVehicleInfo(vehicle_info_); + + // set enabled planner + if (parameters_.enable_shift_pull_out) { + pull_out_planners_.push_back( + std::make_shared(node, parameters, lane_departure_checker_)); + } + if (parameters_.enable_geometric_pull_out) { + pull_out_planners_.push_back( + std::make_shared(node, parameters, getGeometricPullOutParameters())); + } + if (pull_out_planners_.empty()) { + RCLCPP_DEBUG(getLogger(), "Not found enabled planner"); + } } BehaviorModuleOutput PullOutModule::run() @@ -49,14 +68,31 @@ void PullOutModule::onEntry() { RCLCPP_DEBUG(getLogger(), "PULL_OUT onEntry"); current_state_ = BT::NodeStatus::SUCCESS; - updatePullOutStatus(); - // Get arclength to start lane change - const auto current_pose = planner_data_->self_pose->pose; - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); - status_.back_finished = false; - status_.start_distance = arclength_start.length; + // initialize when receiving new route + if ( + last_route_received_time_ == nullptr || + *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { + RCLCPP_INFO(getLogger(), "Receive new route, so reset status"); + resetStatus(); + updatePullOutStatus(); + } + last_route_received_time_ = + std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); + + // for preventing chattering between back and pull_out + if (!status_.back_finished) { + if (last_pull_out_start_update_time_ == nullptr) { + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); + } + const auto elpased_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); + if (elpased_time < parameters_.backward_path_update_duration) { + return; + } + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); + } + + updatePullOutStatus(); } void PullOutModule::onExit() @@ -73,26 +109,24 @@ bool PullOutModule::isExecutionRequested() const return true; } - const bool car_is_stopping = - (util::l2Norm(planner_data_->self_odometry->twist.twist.linear) <= 1.5) ? true : false; + const bool is_stopped = util::l2Norm(planner_data_->self_odometry->twist.twist.linear) < + parameters_.th_arrived_distance; lanelet::Lanelet closest_shoulder_lanelet; - if ( lanelet::utils::query::getClosestLanelet( planner_data_->route_handler->getShoulderLanelets(), planner_data_->self_pose->pose, &closest_shoulder_lanelet) && - car_is_stopping) { + is_stopped) { // Create vehicle footprint - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); + const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); const auto vehicle_footprint = transformVector( local_vehicle_footprint, tier4_autoware_utils::pose2transform(planner_data_->self_pose->pose)); const auto road_lanes = getCurrentLanes(); // check if goal pose is in shoulder lane and distance is long enough for pull out - if (isInLane(closest_shoulder_lanelet, vehicle_footprint) && isLongEnough(road_lanes)) { + if (isInLane(closest_shoulder_lanelet, vehicle_footprint)) { return true; } } @@ -100,253 +134,252 @@ bool PullOutModule::isExecutionRequested() const return false; } -bool PullOutModule::isExecutionReady() const -{ - if (current_state_ == BT::NodeStatus::RUNNING) { - return true; - } - - // TODO(sugahara) move to utility function - const auto road_lanes = getCurrentLanes(); - const auto shoulder_lanes = getPullOutLanes(road_lanes); - - // Find pull_out path - bool found_valid_path, found_safe_path; - PullOutPath selected_path; - std::tie(found_valid_path, found_safe_path) = - getSafePath(shoulder_lanes, check_distance_, selected_path); - - if (found_valid_path && !found_safe_path) { - double back_distance; - if (getBackDistance(shoulder_lanes, check_distance_, selected_path, back_distance)) { - return true; - } - } - return found_safe_path; -} // namespace behavior_path_planner +bool PullOutModule::isExecutionReady() const { return true; } +// this runs only when RUNNING BT::NodeStatus PullOutModule::updateState() { RCLCPP_DEBUG(getLogger(), "PULL_OUT updateState"); - // finish after lane change - if (status_.back_finished && hasFinishedPullOut()) { + if (hasFinishedPullOut()) { current_state_ = BT::NodeStatus::SUCCESS; return current_state_; } - if (status_.is_retreat_path_valid) { - if (hasFinishedBack()) { - status_.back_finished = true; - } - } - current_state_ = BT::NodeStatus::RUNNING; + + checkBackFinished(); + return current_state_; } BehaviorModuleOutput PullOutModule::plan() { - constexpr double RESAMPLE_INTERVAL = 1.0; + BehaviorModuleOutput output; + if (!status_.is_safe) { + return output; + } PathWithLaneId path; - if (status_.is_retreat_path_valid && !status_.is_safe) { - path = util::resamplePathWithSpline(status_.straight_back_path.path, RESAMPLE_INTERVAL); + if (status_.back_finished) { + if (hasFinishedCurrentPath()) { + RCLCPP_INFO(getLogger(), "Increment path index"); + incrementPathIndex(); + } + path = getCurrentPath(); } else { - path = util::resamplePathWithSpline(status_.pull_out_path.path, RESAMPLE_INTERVAL); - status_.back_finished = true; - } - - if (status_.is_retreat_path_valid && status_.back_finished) { - path = util::resamplePathWithSpline(status_.retreat_path.path, RESAMPLE_INTERVAL); + path = status_.backward_path; } - path.drivable_area = status_.pull_out_path.path.drivable_area; - - BehaviorModuleOutput output; output.path = std::make_shared(path); - output.turn_signal_info = calcTurnSignalInfo(status_.pull_out_path.shift_point); + output.turn_signal_info = + calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + + setDebugData(); return output; } -CandidateOutput PullOutModule::planCandidate() const +CandidateOutput PullOutModule::planCandidate() const { return CandidateOutput{}; } + +std::shared_ptr PullOutModule::getCurrentPlanner() const { - // Get lane change lanes - const auto current_lanes = getCurrentLanes(); - const auto shoulder_lanes = getPullOutLanes(current_lanes); + for (const auto & planner : pull_out_planners_) { + if (status_.planner_type == planner->getPlannerType()) { + return planner; + } + } + return nullptr; +} - const auto current_pose = planner_data_->self_pose->pose; +PathWithLaneId PullOutModule::getFullPath() const +{ + const auto pull_out_planner = getCurrentPlanner(); + if (pull_out_planner == nullptr) { + return PathWithLaneId{}; + } - // Find pull out path - bool found_valid_path, found_safe_path; - PullOutPath selected_path; - std::tie(found_valid_path, found_safe_path) = - getSafePath(shoulder_lanes, check_distance_, selected_path); - - if (found_valid_path && !found_safe_path) { - double back_distance; - if (getBackDistance(shoulder_lanes, check_distance_, selected_path, back_distance)) { - bool found_valid_retreat_path, found_safe_retreat_path; - RetreatPath selected_retreat_path; - std::tie(found_valid_retreat_path, found_safe_retreat_path) = - getSafeRetreatPath(shoulder_lanes, check_distance_, selected_retreat_path, back_distance); - // ROS_ERROR("found safe retreat path in plan candidate :%d", found_safe_retreat_path); - if (found_safe_retreat_path == true) { - selected_retreat_path.pull_out_path.path.header = - planner_data_->route_handler->getRouteHeader(); - CandidateOutput output_retreat(selected_retreat_path.pull_out_path.path); - output_retreat.distance_to_path_change = motion_utils::calcSignedArcLength( - selected_retreat_path.pull_out_path.path.points, current_pose.position, - selected_retreat_path.backed_pose.position); - return output_retreat; - } - } + // combine partial pull out path + PathWithLaneId pull_out_path; + for (const auto & partial_path : status_.pull_out_path.partial_paths) { + pull_out_path.points.insert( + pull_out_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - // ROS_ERROR("found safe path in plan candidate :%d", found_safe_path); - selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - CandidateOutput output(selected_path.path); - output.distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, current_pose.position, selected_path.shift_point.start.position); - return output; + if (status_.back_finished) { + // not need backward path or finish it + return pull_out_path; + } + + // concat back_path and pull_out_path and + auto full_path = status_.backward_path; + full_path.points.insert( + full_path.points.end(), pull_out_path.points.begin(), pull_out_path.points.end()); + return full_path; } BehaviorModuleOutput PullOutModule::planWaitingApproval() { - BehaviorModuleOutput out; - const auto common_parameters = planner_data_->parameters; + BehaviorModuleOutput output; const auto current_lanes = getCurrentLanes(); - const auto shoulder_lanes = getPullOutLanes(current_lanes); + const auto shoulder_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); - PathWithLaneId candidate_path; - // Generate drivable area - { - const auto candidate = planCandidate(); - candidate_path = candidate.path_candidate; - lanelet::ConstLanelets lanes; - lanes.insert(lanes.end(), current_lanes.begin(), current_lanes.end()); - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - const double resolution = common_parameters.drivable_area_resolution; - candidate_path.drivable_area = util::generateDrivableArea( - candidate_path, lanes, resolution, common_parameters.vehicle_length, planner_data_); - - updateRTCStatus(candidate.distance_to_path_change); - } - for (size_t i = 1; i < candidate_path.points.size(); i++) { - candidate_path.points.at(i).point.longitudinal_velocity_mps = 0.0; + const auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + auto stop_path = candidate_path; + for (auto & p : stop_path.points) { + p.point.longitudinal_velocity_mps = 0.0; } - out.path = std::make_shared(candidate_path); - out.path_candidate = std::make_shared(candidate_path); + output.path = std::make_shared(stop_path); + output.turn_signal_info = + calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + output.path_candidate = std::make_shared(candidate_path); waitApproval(); + // requset approval when stopped at the corresponding point, so distance is 0 + updateRTCStatus(0.0); - return out; + setDebugData(); + return output; } -void PullOutModule::setParameters(const PullOutParameters & parameters) +void PullOutModule::resetStatus() { - parameters_ = parameters; + PullOutStatus initial_status; + status_ = initial_status; } -void PullOutModule::updatePullOutStatus() +ParallelParkingParameters PullOutModule::getGeometricPullOutParameters() const { - const auto & route_handler = planner_data_->route_handler; - const auto common_parameters = planner_data_->parameters; + ParallelParkingParameters params{}; - const auto current_lanes = getCurrentLanes(); - status_.current_lanes = current_lanes; + params.th_arrived_distance = parameters_.th_arrived_distance; + params.th_stopped_velocity = parameters_.th_stopped_velocity; + params.arc_path_interval = parameters_.arc_path_interval; + params.departing_velocity = parameters_.geometric_pull_out_velocity; + params.departing_lane_departure_margin = parameters_.lane_departure_margin; + params.max_steer_angle = parameters_.pull_out_max_steer_angle; - // Get pull_out lanes - const auto pull_out_lanes = getPullOutLanes(current_lanes); - status_.pull_out_lanes = pull_out_lanes; + return params; +} - const auto current_pose = planner_data_->self_pose->pose; - // const auto current_twist = planner_data_->self_odometry->twist.twist; - // const auto common_parameters = planner_data_->parameters; - - // Find pull_out path - bool found_valid_path, found_safe_path; - PullOutPath selected_path; - std::tie(found_valid_path, found_safe_path) = - getSafePath(pull_out_lanes, check_distance_, selected_path); - - if (found_valid_path && !found_safe_path) { - double back_distance; - if (getBackDistance(pull_out_lanes, check_distance_, selected_path, back_distance)) { - bool found_valid_retreat_path, found_safe_retreat_path; - RetreatPath selected_retreat_path; - std::tie(found_valid_retreat_path, found_safe_retreat_path) = - getSafeRetreatPath(pull_out_lanes, check_distance_, selected_retreat_path, back_distance); - if (found_valid_retreat_path && found_safe_retreat_path) { - status_.is_retreat_path_valid = true; - status_.backed_pose = selected_retreat_path.backed_pose; - status_.retreat_path = selected_retreat_path.pull_out_path; - status_.retreat_path.path.header = planner_data_->route_handler->getRouteHeader(); - status_.straight_back_path = pull_out_utils::getBackPaths( - *route_handler, pull_out_lanes, current_pose, common_parameters, parameters_, - back_distance); +void PullOutModule::incrementPathIndex() +{ + status_.current_path_idx = + std::min(status_.current_path_idx + 1, status_.pull_out_path.partial_paths.size() - 1); +} + +PathWithLaneId PullOutModule::getCurrentPath() const +{ + return status_.pull_out_path.partial_paths.at(status_.current_path_idx); +} + +void PullOutModule::planWithPriorityOnEfficientPath( + const std::vector & start_pose_candidates, const Pose & goal_pose) +{ + status_.is_safe = false; + + for (const auto & planner : pull_out_planners_) { + for (const auto & pull_out_start_pose : start_pose_candidates) { + // plan with each planner + planner->setPlannerData(planner_data_); + const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + if (pull_out_path) { // found safe path + status_.is_safe = true; + status_.pull_out_path = *pull_out_path; + status_.pull_out_start_pose = pull_out_start_pose; + status_.planner_type = planner->getPlannerType(); + break; } + // pull out start pose is not current_pose(index > 0), so need back. + status_.back_finished = false; + } + if (status_.is_safe) { + break; } } +} - // Update status - status_.is_safe = found_safe_path; - status_.pull_out_path = selected_path; - - status_.lane_follow_lane_ids = util::getIds(current_lanes); - status_.pull_out_lane_ids = util::getIds(pull_out_lanes); - - // Generate drivable area - { - lanelet::ConstLanelets lanes; - lanes.insert(lanes.end(), current_lanes.begin(), current_lanes.end()); - lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); - - const double resolution = common_parameters.drivable_area_resolution; - status_.pull_out_path.path.drivable_area = util::generateDrivableArea( - status_.pull_out_path.path, lanes, resolution, common_parameters.vehicle_length, - planner_data_); +void PullOutModule::planWithPriorityOnShortBackDistance( + const std::vector & start_pose_candidates, const Pose & goal_pose) +{ + status_.is_safe = false; + + for (const auto & pull_out_start_pose : start_pose_candidates) { + // plan with each planner + for (const auto & planner : pull_out_planners_) { + planner->setPlannerData(planner_data_); + const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + if (pull_out_path) { // found safe path + status_.is_safe = true; + status_.pull_out_path = *pull_out_path; + status_.pull_out_start_pose = pull_out_start_pose; + status_.planner_type = planner->getPlannerType(); + break; + } + } + if (status_.is_safe) { + break; + } + // pull out start pose is not current_pose(index > 0), so need back. + status_.back_finished = false; } - - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); - status_.start_distance = arclength_start.length; - - status_.pull_out_path.path.header = planner_data_->route_handler->getRouteHeader(); } -PathWithLaneId PullOutModule::getReferencePath() const +void PullOutModule::updatePullOutStatus() { - PathWithLaneId reference_path; - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto goal_pose = planner_data_->route_handler->getGoalPose(); - const auto common_parameters = planner_data_->parameters; - - // Set header - reference_path.header = route_handler->getRouteHeader(); + const auto & common_parameters = planner_data_->parameters; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & goal_pose = planner_data_->route_handler->getGoalPose(); const auto current_lanes = getCurrentLanes(); - const auto pull_out_lanes = getPullOutLanes(current_lanes); + status_.current_lanes = current_lanes; + + // Get pull_out lanes + const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); + status_.pull_out_lanes = pull_out_lanes; - if (current_lanes.empty()) { - return reference_path; + // search pull out start candidates backward + std::vector start_pose_candidates; + { + if (parameters_.enable_back) { + // the first element is current_pose + start_pose_candidates = searchBackedPoses(); + } else { + // pull_out_start candidate is only current pose + start_pose_candidates.push_back(current_pose); + } } - reference_path = util::getCenterLinePath( - *route_handler, pull_out_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); + if (parameters_.search_priority == "efficient_path") { + planWithPriorityOnEfficientPath(start_pose_candidates, goal_pose); + } else if (parameters_.search_priority == "short_back_distance") { + planWithPriorityOnShortBackDistance(start_pose_candidates, goal_pose); + } else { + RCLCPP_ERROR( + getLogger(), + "search_priority should be efficient_path or short_back_distance, but %s is given.", + parameters_.search_priority.c_str()); + } + + if (!status_.is_safe) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Not found safe pull out path"); + status_.is_safe = false; + return; + } - reference_path = util::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_.after_pull_out_straight_distance, - common_parameters.minimum_pull_out_length, parameters_.before_pull_out_straight_distance, - parameters_.deceleration_interval, goal_pose); + checkBackFinished(); + if (!status_.back_finished) { + status_.backward_path = pull_out_utils::getBackwardPath( + *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose, + parameters_.backward_velocity); + status_.backward_path.drivable_area = util::generateDrivableArea( + status_.backward_path, pull_out_lanes, common_parameters.drivable_area_resolution, + common_parameters.vehicle_length, planner_data_); + } - reference_path.drivable_area = util::generateDrivableArea( - reference_path, pull_out_lanes, common_parameters.drivable_area_resolution, - common_parameters.vehicle_length, planner_data_); - return reference_path; + // Update status + status_.lane_follow_lane_ids = util::getIds(current_lanes); + status_.pull_out_lane_ids = util::getIds(pull_out_lanes); } lanelet::ConstLanelets PullOutModule::getCurrentLanes() const @@ -368,245 +401,51 @@ lanelet::ConstLanelets PullOutModule::getCurrentLanes() const common_parameters.forward_path_length); } -// getShoulderLanesOnCurrentPose? -lanelet::ConstLanelets PullOutModule::getPullOutLanes( - const lanelet::ConstLanelets & current_lanes) const +// make this class? +std::vector PullOutModule::searchBackedPoses() { - lanelet::ConstLanelets shoulder_lanes; - const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; - lanelet::ConstLanelet shoulder_lane; - - if (current_lanes.empty()) { - return shoulder_lanes; - } - - // Get pull out lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet( - current_lanes, planner_data_->self_pose->pose, ¤t_lane); - - if (route_handler->getPullOutStart( - route_handler->getShoulderLanelets(), &shoulder_lane, current_pose, - planner_data_->parameters.vehicle_width)) { - shoulder_lanes = route_handler->getShoulderLaneletSequence( - shoulder_lane, current_pose, pull_out_lane_length_, pull_out_lane_length_); - - } else { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "getPullOverTarget didn't work"); - shoulder_lanes.clear(); - } - - return shoulder_lanes; -} - -std::pair PullOutModule::getSafePath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - PullOutPath & safe_path) const -{ - std::vector valid_paths; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; - const auto road_lanes = getCurrentLanes(); - - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); - - if (!pull_out_lanes.empty()) { - // find candidate paths - const auto pull_out_paths = pull_out_utils::getPullOutPaths( - *route_handler, road_lanes, pull_out_lanes, current_pose, common_parameters, parameters_); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!pull_out_paths.empty()) { - const auto & longest_path = pull_out_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.pull_out_length; - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, pull_out_lanes, check_distance_with_path); - } - - // select valid path - valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, - route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose()); - - if (valid_paths.empty()) { - RCLCPP_DEBUG(getLogger(), "valid path is empty"); - return std::make_pair(false, false); - } - // select safe path - bool found_safe_path = pull_out_utils::selectSafePath( - valid_paths, road_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, local_vehicle_footprint, - &safe_path); - - return std::make_pair(true, found_safe_path); - } - return std::make_pair(false, false); -} - -std::pair PullOutModule::getSafeRetreatPath( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - RetreatPath & safe_retreat_path, double & back_distance) const -{ - std::vector valid_paths; - PullOutPath safe_path; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; - - const auto road_lanes = getCurrentLanes(); - - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); + const auto current_lanes = getCurrentLanes(); + const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); - lanelet::ConstLanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - pull_out_lanes, current_pose, &closest_shoulder_lanelet)) { - // RCLCPP_ERROR_THROTTLE(getLogger(), clock, 1000, "Failed to find closest lane!"); - } + // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose); - - const auto shoulder_line_path = route_handler->getCenterLinePath( - pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, - arc_position_pose.length + pull_out_lane_length_); - const auto idx = motion_utils::findNearestIndex(shoulder_line_path.points, current_pose); - const auto yaw_shoulder_lane = - tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); - - const auto backed_pose = - pull_out_utils::getBackedPose(current_pose, yaw_shoulder_lane, back_distance); - - if (!pull_out_lanes.empty()) { - // find candidate paths - const auto pull_out_paths = pull_out_utils::getPullOutPaths( - *route_handler, road_lanes, pull_out_lanes, backed_pose, common_parameters, parameters_, - true); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!pull_out_paths.empty()) { - const auto & longest_path = pull_out_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.pull_out_length; - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, pull_out_lanes, check_distance_with_path); - } - - // select valid path - valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, - route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose()); - - if (valid_paths.empty()) { - return std::make_pair(false, false); - } - // select safe path - bool found_safe_path = pull_out_utils::selectSafePath( - valid_paths, road_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, local_vehicle_footprint, - &safe_path); - safe_retreat_path.pull_out_path = safe_path; - safe_retreat_path.backed_pose = backed_pose; - - return std::make_pair(true, found_safe_path); + const double check_distance = parameters_.max_back_distance + 30.0; // buffer + auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + pull_out_lanes, arc_position_pose.length - check_distance, + arc_position_pose.length + check_distance); + + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : backward_shoulder_path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); } - return std::make_pair(false, false); -} - -bool PullOutModule::getBackDistance( - const lanelet::ConstLanelets & pull_out_lanes, const double check_distance, - PullOutPath & safe_path, double & back_distance) const -{ - std::vector valid_paths; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; - const double back_distance_search_resolution = 1; - const double maximum_back_distance = 15; - - const auto road_lanes = getCurrentLanes(); - - const auto vehicle_info = getVehicleInfo(planner_data_->parameters); - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info); - - double yaw_shoulder_lane; - { - lanelet::ConstLanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - pull_out_lanes, current_pose, &closest_shoulder_lanelet)) { - return false; + // check collision between footprint and onject at the backed pose + const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); + std::vector backed_poses; + for (double back_distance = 0.0; back_distance <= parameters_.max_back_distance; + back_distance += parameters_.backward_search_resolution) { + const auto backed_pose = calcLongitudinalOffsetPose( + backward_shoulder_path.points, current_pose.position, -back_distance); + if (!backed_pose) { + continue; } - const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose); - - const auto shoulder_line_path = route_handler->getCenterLinePath( - pull_out_lanes, arc_position_pose.length - pull_out_lane_length_, - arc_position_pose.length + pull_out_lane_length_); - const auto idx = motion_utils::findNearestIndex(shoulder_line_path.points, current_pose); - yaw_shoulder_lane = tf2::getYaw(shoulder_line_path.points.at(*idx).point.pose.orientation); - } - - for (double current_back_distance = back_distance_search_resolution; - current_back_distance <= maximum_back_distance; - current_back_distance += back_distance_search_resolution) { - if (!pull_out_lanes.empty()) { - const auto backed_pose = - pull_out_utils::getBackedPose(current_pose, yaw_shoulder_lane, current_back_distance); - - // find candidate paths - const auto pull_out_paths = pull_out_utils::getPullOutPaths( - *route_handler, road_lanes, pull_out_lanes, backed_pose, common_parameters, parameters_, - true); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!pull_out_paths.empty()) { - const auto & longest_path = pull_out_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance + longest_path.preparation_length + longest_path.pull_out_length; - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, pull_out_lanes, check_distance_with_path); - } - - // select valid path - valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, - route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose()); - - if (valid_paths.empty()) { - continue; - } - // select safe path - bool found_safe_path = pull_out_utils::selectSafePath( - valid_paths, road_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, local_vehicle_footprint, - &safe_path); - if (found_safe_path) { - back_distance = current_back_distance; - return found_safe_path; - } + if (util::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, *(planner_data_->dynamic_object), + parameters_.collision_check_margin)) { + break; // poses behind this has a collision, so break. } + + backed_poses.push_back(*backed_pose); } - return false; + return backed_poses; } bool PullOutModule::isInLane( const lanelet::ConstLanelet & candidate_lanelet, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint) const + const tier4_autoware_utils::LinearRing2d & vehicle_footprint) { for (const auto & point : vehicle_footprint) { if (boost::geometry::within(point, candidate_lanelet.polygon2d().basicPolygon())) { @@ -617,150 +456,127 @@ bool PullOutModule::isInLane( return false; } -bool PullOutModule::isLongEnough(const lanelet::ConstLanelets & lanelets) const +bool PullOutModule::hasFinishedPullOut() const { - PathShifter path_shifter; - const double maximum_jerk = parameters_.maximum_lateral_jerk; - const double pull_out_velocity = parameters_.minimum_pull_out_velocity; + if (!status_.back_finished) { + return false; + } + + // check ego car is close enough to goal pose const auto current_pose = planner_data_->self_pose->pose; - const auto goal_pose = planner_data_->route_handler->getGoalPose(); - const double distance_before_pull_out = parameters_.before_pull_out_straight_distance; - const double distance_after_pull_out = parameters_.after_pull_out_straight_distance; - const double distance_to_road_center = - lanelet::utils::getArcCoordinates(lanelets, planner_data_->self_pose->pose).distance; - - // calculate minimum pull_out distance at pull_out velocity, - // maximum jerk and calculated side offset - const double pull_out_distance_min = path_shifter.calcLongitudinalDistFromJerk( - abs(distance_to_road_center), maximum_jerk, pull_out_velocity); - const double pull_out_total_distance_min = - distance_before_pull_out + pull_out_distance_min + distance_after_pull_out; - const double distance_to_goal_on_road_lane = - util::getSignedDistance(current_pose, goal_pose, lanelets); - - return distance_to_goal_on_road_lane > pull_out_total_distance_min; -} + const auto arclength_current = + lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); + const auto arclength_pull_out_end = + lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); -bool PullOutModule::isSafe() const { return status_.is_safe; } + // has passed pull out end point + return arclength_current.length - arclength_pull_out_end.length > + parameters_.pull_out_finish_judge_buffer; +} -bool PullOutModule::isNearEndOfLane() const +void PullOutModule::checkBackFinished() { + // check ego car is close enough to pull out start pose const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; - const double threshold = 5 + common_parameters.minimum_pull_over_length; + const auto distance = + tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); - return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < - threshold; -} + const bool is_near = distance < parameters_.th_arrived_distance; + const double ego_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); + const bool is_stopped = ego_vel < parameters_.th_stopped_velocity; -bool PullOutModule::isCurrentSpeedLow() const -{ - const auto current_twist = planner_data_->self_odometry->twist.twist; - const double threshold_kmph = 10; - return util::l2Norm(current_twist.linear) < threshold_kmph * 1000 / 3600; + if (!status_.back_finished && is_near && is_stopped) { + RCLCPP_INFO(getLogger(), "back finished"); + status_.back_finished = true; + + // requst pull_out approval + waitApproval(); + removeRTCStatus(); + uuid_ = generateUUID(); + // requset approval when stopped at the corresponding point, so distance is 0 + updateRTCStatus(0.0); + current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop + } } -bool PullOutModule::hasFinishedPullOut() const +bool PullOutModule::isStopped() { - // check ego car is close enough to goal pose - const auto current_pose = planner_data_->self_pose->pose; - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); - const auto arclength_shift_end = - lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.shift_point.end); - const bool car_is_on_goal = (arclength_shift_end.length - arclength_current.length < - parameters_.pull_out_finish_judge_buffer) - ? true - : false; - - return car_is_on_goal; + odometry_buffer_.push_back(planner_data_->self_odometry); + // Delete old data in buffer + while (rclcpp::ok()) { + const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - + rclcpp::Time(odometry_buffer_.front()->header.stamp); + if (time_diff.seconds() < parameters_.th_stopped_time) { + break; + } + odometry_buffer_.pop_front(); + } + bool is_stopped = true; + for (const auto & odometry : odometry_buffer_) { + const double ego_vel = util::l2Norm(odometry->twist.twist.linear); + if (ego_vel > parameters_.th_stopped_velocity) { + is_stopped = false; + break; + } + } + return is_stopped; } -bool PullOutModule::hasFinishedBack() const +bool PullOutModule::hasFinishedCurrentPath() { - // check ego car is close enough to goal pose - const auto current_pose = planner_data_->self_pose->pose; - const auto backed_pose = status_.backed_pose; - const auto distance = tier4_autoware_utils::calcDistance2d(current_pose, backed_pose); + const auto current_path = getCurrentPath(); + const auto current_path_end = current_path.points.back(); + const auto self_pose = planner_data_->self_pose->pose; + const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < + parameters_.th_arrived_distance; - return distance < 1; + return is_near_target && isStopped(); } -TurnSignalInfo PullOutModule::calcTurnSignalInfo(const ShiftPoint & shift_point) const +TurnSignalInfo PullOutModule::calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const { TurnSignalInfo turn_signal; - if (status_.is_retreat_path_valid && !status_.back_finished) { + // turn hazard light when backward driving + if (!status_.back_finished) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; turn_signal.signal_distance = - tier4_autoware_utils::calcDistance2d(status_.backed_pose, planner_data_->self_pose->pose); + tier4_autoware_utils::calcDistance2d(start_pose, planner_data_->self_pose->pose); return turn_signal; } - const auto current_lanes = getCurrentLanes(); - const auto pull_out_lanes = getPullOutLanes(current_lanes); - const double turn_signal_on_threshold = 30; - const double turn_signal_off_threshold = -3; - const double turn_hazard_on_threshold = 3; - - // calculate distance to pull_out start on current lanes - double distance_to_pull_out_start; - { - const auto pull_out_start = shift_point.start; - const auto arc_position_pull_out_start = - lanelet::utils::getArcCoordinates(current_lanes, pull_out_start); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(current_lanes, planner_data_->self_pose->pose); - distance_to_pull_out_start = - arc_position_pull_out_start.length - arc_position_current_pose.length; - } - // calculate distance to pull_out end on target lanes - double distance_to_pull_out_end; - { - const auto pull_out_end = shift_point.end; - const auto arc_position_pull_out_end = - lanelet::utils::getArcCoordinates(pull_out_lanes, pull_out_end); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(pull_out_lanes, planner_data_->self_pose->pose); - distance_to_pull_out_end = arc_position_pull_out_end.length - arc_position_current_pose.length; - } - - // calculate distance to pull_out start on target lanes - double distance_to_target_pose; - { - const auto arc_position_target_pose = lanelet::utils::getArcCoordinates( - pull_out_lanes, planner_data_->route_handler->getGoalPose()); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(pull_out_lanes, planner_data_->self_pose->pose); - distance_to_target_pose = arc_position_target_pose.length - arc_position_current_pose.length; - } - - if (distance_to_pull_out_start < turn_signal_on_threshold) { + const auto current_lanes = getCurrentLanes(); + const auto arc_position_current_pose = + lanelet::utils::getArcCoordinates(current_lanes, planner_data_->self_pose->pose); + const auto arc_position_pull_out_end = lanelet::utils::getArcCoordinates(current_lanes, end_pose); + const double distance_from_pull_out_end = + arc_position_current_pose.length - arc_position_pull_out_end.length; + + // turn on right signal until passing pull_out end point + const double turn_signal_off_buffer = std::min(parameters_.pull_out_finish_judge_buffer, 3.0); + if (distance_from_pull_out_end < turn_signal_off_buffer) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - if (distance_to_pull_out_end < turn_signal_off_threshold) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; - if (distance_to_target_pose < turn_hazard_on_threshold) { - turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - } - } + } else { + turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; } - turn_signal.signal_distance = distance_to_pull_out_end; + + turn_signal.signal_distance = -distance_from_pull_out_end + turn_signal_off_buffer; return turn_signal; } -vehicle_info_util::VehicleInfo PullOutModule::getVehicleInfo( - const BehaviorPathPlannerParameters & parameters) const +void PullOutModule::setDebugData() const { - vehicle_info_util::VehicleInfo vehicle_info; - vehicle_info.front_overhang_m = parameters.front_overhang; - vehicle_info.wheel_base_m = parameters.wheel_base; - vehicle_info.rear_overhang_m = parameters.rear_overhang; - vehicle_info.wheel_tread_m = parameters.wheel_tread; - vehicle_info.left_overhang_m = parameters.left_over_hang; - vehicle_info.right_overhang_m = parameters.right_over_hang; - return vehicle_info; -} + using marker_utils::createPathMarkerArray; + using marker_utils::createPoseMarkerArray; + + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; + debug_marker_.markers.clear(); + add(createPoseMarkerArray(status_.pull_out_start_pose, "pull_out_start_pose", 0, 0.9, 0.3, 0.3)); + add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp new file mode 100644 index 0000000000000..7371ad99fb80d --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -0,0 +1,259 @@ +// 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 "behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp" + +#include "behavior_path_planner/path_utilities.hpp" +#include "behavior_path_planner/scene_module/pull_out/util.hpp" + +#include +#include + +using lanelet::utils::getArcCoordinates; +using motion_utils::findNearestIndex; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +namespace behavior_path_planner +{ +using pull_out_utils::combineReferencePath; +using pull_out_utils::getPullOutLanes; + +ShiftPullOut::ShiftPullOut( + rclcpp::Node & node, const PullOutParameters & parameters, + std::shared_ptr & lane_departure_checker) +: PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker} +{ +} + +boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +{ + const auto & route_handler = planner_data_->route_handler; + const auto & common_parameters = planner_data_->parameters; + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & road_lanes = util::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_pose->pose; + const auto & shoulder_lanes = getPullOutLanes(road_lanes, planner_data_); + if (shoulder_lanes.empty()) { + return boost::none; + } + + lanelet::ConstLanelets lanes; + lanes.insert(lanes.end(), road_lanes.begin(), road_lanes.end()); + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + // find candidate paths + auto pull_out_paths = calcPullOutPaths( + *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters, + parameters_); + if (pull_out_paths.empty()) { + return boost::none; + } + + // extract objects in shoulder lane for collision check + const auto shoulder_lane_objects = + util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes); + + // get safe path + for (auto & pull_out_path : pull_out_paths) { + auto & shift_path = + pull_out_path.partial_paths.front(); // shift path is not separate but only one. + + // check lane_depature and collsion with path between current to pull_out_end + PathWithLaneId path_current_to_shift_end; + { + const auto current_idx = findNearestIndex(shift_path.points, current_pose); + const auto pull_out_end_idx = findNearestIndex(shift_path.points, pull_out_path.end_pose); + path_current_to_shift_end.points.insert( + path_current_to_shift_end.points.begin(), shift_path.points.begin() + *current_idx, + shift_path.points.begin() + *pull_out_end_idx + 1); + } + + // check lane departure + if (lane_departure_checker_->checkPathWillLeaveLane(lanes, path_current_to_shift_end)) { + continue; + } + + // check collision + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, path_current_to_shift_end, shoulder_lane_objects, + parameters_.collision_check_margin)) { + continue; + } + + // Generate drivable area + const double resolution = common_parameters.drivable_area_resolution; + shift_path.drivable_area = util::generateDrivableArea( + shift_path, lanes, resolution, common_parameters.vehicle_length, planner_data_); + + shift_path.header = planner_data_->route_handler->getRouteHeader(); + + return pull_out_path; + } + + return boost::none; +} + +std::vector ShiftPullOut::calcPullOutPaths( + const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter) +{ + std::vector candidate_paths; + + if (road_lanes.empty() || shoulder_lanes.empty()) { + return candidate_paths; + } + + // rename parameter + const double backward_path_length = common_parameter.backward_path_length; + const double shift_pull_out_velocity = parameter.shift_pull_out_velocity; + const double before_pull_out_straight_distance = parameter.before_pull_out_straight_distance; + const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance; + const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; + const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; + const int pull_out_sampling_num = parameter.pull_out_sampling_num; + const double jerk_resolution = + std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_out_sampling_num; + + for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; + lateral_jerk += jerk_resolution) { + PathShifter path_shifter; + const double distance_to_road_center = getArcCoordinates(road_lanes, start_pose).distance; + const double pull_out_distance = std::max( + path_shifter.calcLongitudinalDistFromJerk( + abs(distance_to_road_center), lateral_jerk, shift_pull_out_velocity), + minimum_shift_pull_out_distance); + + // check has enough distance + const double pull_out_total_distance = before_pull_out_straight_distance + pull_out_distance; + const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); + if (!hasEnoughDistance( + pull_out_total_distance, road_lanes, start_pose, is_in_goal_route_section, goal_pose)) { + continue; + } + + PathWithLaneId shoulder_reference_path; + { + const auto arc_position = getArcCoordinates(shoulder_lanes, start_pose); + const double s_start = arc_position.length - backward_path_length; + double s_end = arc_position.length + before_pull_out_straight_distance; + s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); + shoulder_reference_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end); + + // lateral shift to start_pose + for (auto & p : shoulder_reference_path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, arc_position.distance, 0); + } + } + + PathWithLaneId road_lane_reference_path; + { + const lanelet::ArcCoordinates arc_position_shift = + getArcCoordinates(road_lanes, shoulder_reference_path.points.back().point.pose); + const lanelet::ArcCoordinates arc_position_goal = getArcCoordinates(road_lanes, goal_pose); + + double s_start = arc_position_shift.length; + double s_end = arc_position_goal.length; + road_lane_reference_path = route_handler.getCenterLinePath(road_lanes, s_start, s_end); + } + + // get shift point start/end + const auto shift_point_start = shoulder_reference_path.points.back(); + const auto shift_point_end = [&]() { + const auto arc_position_shift_start = + lanelet::utils::getArcCoordinates(road_lanes, shift_point_start.point.pose); + const double s_start = arc_position_shift_start.length + pull_out_distance; + const double s_end = s_start + std::numeric_limits::epsilon(); + const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true); + return path.points.front(); + }(); + + ShiftPoint shift_point; + { + shift_point.start = shift_point_start.point.pose; + shift_point.end = shift_point_end.point.pose; + shift_point.length = distance_to_road_center; + } + path_shifter.addShiftPoint(shift_point); + path_shifter.setPath(road_lane_reference_path); + + // offset front side + ShiftedPath shifted_path; + const bool offset_back = false; + if (!path_shifter.generate(&shifted_path, offset_back)) { + continue; + } + + const auto pull_out_end_idx = + findNearestIndex(shifted_path.path.points, shift_point_end.point.pose); + const auto goal_idx = findNearestIndex(shifted_path.path.points, goal_pose); + + if (pull_out_end_idx && goal_idx) { + // set velocity + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *pull_out_end_idx) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); + continue; + } else if (i > *goal_idx) { + point.point.longitudinal_velocity_mps = 0.0; + continue; + } + } + + PullOutPath candidate_path; + const auto combined_path = combineReferencePath(shoulder_reference_path, shifted_path.path); + candidate_path.partial_paths.push_back(util::resamplePathWithSpline(combined_path, 1.0)); + candidate_path.start_pose = shift_point.start; + candidate_path.end_pose = shift_point.end; + + // add goal pose because resampling removes it + // but this point will be removed by SmoothGoalConnection again + PathPointWithLaneId goal_path_point = shifted_path.path.points.back(); + goal_path_point.point.pose = goal_pose; + goal_path_point.point.longitudinal_velocity_mps = 0.0; + candidate_path.partial_paths.front().points.push_back(goal_path_point); + candidate_paths.push_back(candidate_path); + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), + "lane change end idx not found on target path."); + continue; + } + } + + return candidate_paths; +} + +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 > util::getDistanceToEndOfLane(current_pose, road_lanes)) { + return false; + } + + // current_lanes include goal's lane + if ( + is_in_goal_route_section && + pull_out_total_distance > util::getSignedDistance(current_pose, goal_pose, road_lanes)) { + return false; + } + + return true; +} + +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp index 835f83f811506..7699a58e02ff1 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp @@ -44,261 +44,42 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa // skip overlapping point path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - return path; + return util::removeOverlappingPoints(path); } -bool isPathInLanelets4pullover( - const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets) +PathWithLaneId getBackwardPath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes, + const Pose & current_pose, const Pose & backed_pose, const double velocity) { - for (const auto & pt : path.points) { - bool is_in_lanelet = false; - const lanelet::BasicPoint2d p(pt.point.pose.position.x, pt.point.pose.position.y); - for (const auto & llt : original_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 1)) { - is_in_lanelet = true; - } - } - for (const auto & llt : target_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 1)) { - is_in_lanelet = true; - } - } - if (!is_in_lanelet) { - return false; - } - } - return true; -} - -std::vector getPullOutPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanelets, - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter, - const bool is_retreat_path) -{ - std::vector candidate_paths; - - if (road_lanelets.empty() || shoulder_lanelets.empty()) { - return candidate_paths; - } - // rename parameter - const double backward_path_length = common_parameter.backward_path_length; - const double forward_path_length = common_parameter.forward_path_length; - const double minimum_pull_out_velocity = parameter.minimum_pull_out_velocity; - const double before_pull_out_straight_distance = parameter.before_pull_out_straight_distance; - const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; - const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; - const int pull_out_sampling_num = parameter.pull_out_sampling_num; - const double jerk_resolution = - std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_out_sampling_num; - // maximum lateral jerk is set on retreat path - double initial_lateral_jerk = is_retreat_path ? maximum_lateral_jerk : minimum_lateral_jerk; - - for (double lateral_jerk = initial_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; - lateral_jerk += jerk_resolution) { - PathWithLaneId reference_path; - PathShifter path_shifter; - ShiftedPath shifted_path; - const double v1 = minimum_pull_out_velocity; - - const double distance_to_road_center = - lanelet::utils::getArcCoordinates(road_lanelets, pose).distance; - - const double distance_to_shoulder_center = - lanelet::utils::getArcCoordinates(shoulder_lanelets, pose).distance; - - double pull_out_distance = - path_shifter.calcLongitudinalDistFromJerk(abs(distance_to_road_center), lateral_jerk, v1); - - PathWithLaneId reference_path1; - { - const auto arc_position = lanelet::utils::getArcCoordinates(shoulder_lanelets, pose); - const double s_start = arc_position.length - backward_path_length; - double s_end = arc_position.length + before_pull_out_straight_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - reference_path1 = route_handler.getCenterLinePath(shoulder_lanelets, s_start, s_end); - } - for (auto & point : reference_path1.points) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(minimum_pull_out_velocity)); - } - - // Apply shifting before shift - for (size_t i = 0; i < reference_path1.points.size(); ++i) { - { - if (fabs(distance_to_shoulder_center) < 1.0e-8) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "no offset from current lane center."); - } - - auto & p = reference_path1.points.at(i).point.pose; - double yaw = tf2::getYaw(p.orientation); - p.position.x -= std::sin(yaw) * distance_to_shoulder_center; - p.position.y += std::cos(yaw) * distance_to_shoulder_center; - } - } - - PathWithLaneId reference_path2; - { - const auto arc_position_goal = - lanelet::utils::getArcCoordinates(shoulder_lanelets, route_handler.getGoalPose()); - const auto arc_position_ref1_back = - lanelet::utils::getArcCoordinates(road_lanelets, reference_path1.points.back().point.pose); - double s_start = arc_position_ref1_back.length + pull_out_distance; - double s_end = arc_position_goal.length; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - reference_path2 = route_handler.getCenterLinePath(road_lanelets, s_start, s_end); - } - - if (reference_path1.points.empty() || reference_path2.points.empty()) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "reference path is empty!! something wrong..."); - continue; - } - - PullOutPath candidate_path; - // candidate_path.acceleration = acceleration; - candidate_path.preparation_length = before_pull_out_straight_distance; - candidate_path.pull_out_length = pull_out_distance; - PathWithLaneId target_lane_reference_path; - { - const lanelet::ArcCoordinates pull_out_start_arc_position = - lanelet::utils::getArcCoordinates(road_lanelets, reference_path1.points.back().point.pose); - double s_start = pull_out_start_arc_position.length; - double s_end = s_start + pull_out_distance + forward_path_length; - target_lane_reference_path = route_handler.getCenterLinePath(road_lanelets, s_start, s_end); - } - - ShiftPoint shift_point; - { - const Pose pull_out_start_on_shoulder_lane = reference_path1.points.back().point.pose; - const Pose pull_out_end_on_road_lane = reference_path2.points.front().point.pose; - shift_point.start = pull_out_start_on_shoulder_lane; - shift_point.end = pull_out_end_on_road_lane; - - shift_point.length = distance_to_road_center; - } - path_shifter.addShiftPoint(shift_point); - path_shifter.setPath(target_lane_reference_path); - - // offset front side - bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) { - // ROS_ERROR("failed to generate shifted path."); - continue; - } - - const auto pull_out_end_idx = motion_utils::findNearestIndex( - shifted_path.path.points, reference_path2.points.front().point.pose); - - const auto goal_idx = - motion_utils::findNearestIndex(shifted_path.path.points, route_handler.getGoalPose()); - - if (pull_out_end_idx && goal_idx) { - const auto distance_pull_out_end_to_goal = tier4_autoware_utils::calcDistance2d( - shifted_path.path.points.at(*pull_out_end_idx).point.pose, - shifted_path.path.points.at(*goal_idx).point.pose); - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *pull_out_end_idx) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - reference_path1.points.back().point.longitudinal_velocity_mps); - continue; - } else if (i > *goal_idx) { - point.point.longitudinal_velocity_mps = 0.0; - continue; - } - - auto distance_to_goal = tier4_autoware_utils::calcDistance2d( - point.point.pose, shifted_path.path.points.at(*goal_idx).point.pose); - point.point.longitudinal_velocity_mps = std::min( - minimum_pull_out_velocity, - std::max( - 0.0, (distance_to_goal / distance_pull_out_end_to_goal * minimum_pull_out_velocity))); - point.lane_ids = reference_path2.points.front().lane_ids; - } - candidate_path.path = combineReferencePath(reference_path1, shifted_path.path); - candidate_path.shifted_path = shifted_path; - candidate_path.shift_point = shift_point; - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "lane change end idx not found on target path."); - continue; - } - - // check candidate path is in lanelet - if (!isPathInLanelets4pullover(candidate_path.path, road_lanelets, shoulder_lanelets)) { - continue; - } - - // ROS_ERROR("candidate path is push backed"); - candidate_paths.push_back(candidate_path); - } - - return candidate_paths; -} - -PullOutPath getBackPaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanelets, - const Pose & pose, const BehaviorPathPlannerParameters & common_parameter, - [[maybe_unused]] const PullOutParameters & parameter, [[maybe_unused]] const double back_distance) -{ - PathShifter path_shifter; - ShiftedPath shifted_path; + const auto current_pose_arc_coords = + lanelet::utils::getArcCoordinates(shoulder_lanes, current_pose); + const auto backed_pose_arc_coords = + lanelet::utils::getArcCoordinates(shoulder_lanes, backed_pose); - // rename parameter - const double backward_path_length = common_parameter.backward_path_length; + const double s_start = backed_pose_arc_coords.length; + const double s_end = current_pose_arc_coords.length; - const double distance_to_shoulder_center = - lanelet::utils::getArcCoordinates(shoulder_lanelets, pose).distance; - - PathWithLaneId reference_path1; + PathWithLaneId backward_path; { - const auto arc_position = lanelet::utils::getArcCoordinates(shoulder_lanelets, pose); - const double s_start = arc_position.length + backward_path_length; - double s_end = s_start - 50; - reference_path1 = route_handler.getCenterLinePath(shoulder_lanelets, s_end, s_start); - // ROS_ERROR("ref1 s_start:%f s_end%f", s_start, s_end); - for (auto & point : reference_path1.points) { - // auto arc_length = - // lanelet::utils::getArcCoordinates(shoulder_lanelets, point.point.pose).length; - point.point.longitudinal_velocity_mps = -5; - // ROS_ERROR("back_distance:%f", back_distance); - // if (arc_position.length - arc_length > back_distance) { - // point.point.longitudinal_velocity_mps = 0; - // } - } - // std::reverse(reference_path1.points.begin(), reference_path1.points.end()); - // reference_path1.points.front().point.longitudinal_velocity_mps = 0; - } + // forward center line path + backward_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end, true); - // Apply shifting before shift - for (size_t i = 0; i < reference_path1.points.size(); ++i) { - { - if (fabs(distance_to_shoulder_center) < 1.0e-8) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "no offset from current lane center."); - continue; - } + // backward center line path + std::reverse(backward_path.points.begin(), backward_path.points.end()); + for (auto & p : backward_path.points) { + p.point.longitudinal_velocity_mps = velocity; + } + backward_path.points.back().point.longitudinal_velocity_mps = 0.0; - auto & p = reference_path1.points.at(i).point.pose; - double yaw = tf2::getYaw(p.orientation); - p.position.x -= std::sin(yaw) * distance_to_shoulder_center; - p.position.y += std::cos(yaw) * distance_to_shoulder_center; + // lateral shift to current_pose + const double lateral_distance_to_shoulder_center = current_pose_arc_coords.distance; + for (size_t i = 0; i < backward_path.points.size(); ++i) { + auto & p = backward_path.points.at(i).point.pose; + p = tier4_autoware_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); } } - PullOutPath candidate_path; - - candidate_path.path = reference_path1; - - return candidate_path; + return backward_path; } Pose getBackedPose( @@ -312,172 +93,34 @@ Pose getBackedPose( return backed_pose; } -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose) -{ - std::vector available_paths; - - for (const auto & path : paths) { - if (hasEnoughDistance( - path, road_lanes, shoulder_lanes, current_pose, isInGoalRouteSection, goal_pose, - overall_graphs)) { - available_paths.push_back(path); - } - } - - return available_paths; -} - -bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, - [[maybe_unused]] const Pose & current_pose, [[maybe_unused]] const Twist & current_twist, - [[maybe_unused]] const double vehicle_width, const PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, PullOutPath * selected_path) -{ - const bool use_dynamic_object = ros_parameters.use_dynamic_object; - for (const auto & path : paths) { - if (isPullOutPathSafe( - path, road_lanes, shoulder_lanes, dynamic_objects, ros_parameters, - local_vehicle_footprint, true, use_dynamic_object)) { - *selected_path = path; - return true; - } - } - - // set first path for force pullover if no valid path found - if (!paths.empty()) { - *selected_path = paths.front(); - return false; - } - - return false; -} - -bool hasEnoughDistance( - const PullOutPath & path, const lanelet::ConstLanelets & road_lanes, - [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - [[maybe_unused]] const lanelet::routing::RoutingGraphContainer & overall_graphs) +// getShoulderLanesOnCurrentPose +lanelet::ConstLanelets getPullOutLanes( + const lanelet::ConstLanelets & current_lanes, + const std::shared_ptr & planner_data) { - const double pull_out_prepare_distance = path.preparation_length; - const double pull_out_distance = path.pull_out_length; - const double pull_out_total_distance = pull_out_prepare_distance + pull_out_distance; - - if (pull_out_total_distance > util::getDistanceToEndOfLane(current_pose, road_lanes)) { - return false; - } - - // if (pull_out_total_distance > - // util::getDistanceToNextIntersection(current_pose, current_lanes)) { - // return false; - // } - - if ( - isInGoalRouteSection && - pull_out_total_distance > util::getSignedDistance(current_pose, goal_pose, road_lanes)) { - return false; - } - - // if ( - // pull_out_total_distance > - // util::getDistanceToCrosswalk(current_pose, current_lanes, overall_graphs)) { - // return false; - // } - return true; -} - -bool isPullOutPathSafe( - const behavior_path_planner::PullOutPath & path, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const PullOutParameters & ros_parameters, - const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const bool use_buffer, - const bool use_dynamic_object) -{ - // TODO(sugahara) check road lanes safety and output road lanes safety - // and shoulder lanes safety respectively - if (path.path.points.empty()) { - return false; - } - if (shoulder_lanes.empty() || road_lanes.empty()) { - return false; - } - if (dynamic_objects == nullptr) { - return true; + const double pull_out_lane_length = 200.0; + lanelet::ConstLanelets shoulder_lanes; + const auto & route_handler = planner_data->route_handler; + const auto current_pose = planner_data->self_pose->pose; + lanelet::ConstLanelet shoulder_lane; + + if (current_lanes.empty()) { + return shoulder_lanes; } - const double min_thresh = ros_parameters.min_stop_distance; + // Get pull out lanes + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet( + current_lanes, planner_data->self_pose->pose, ¤t_lane); - double buffer; - // double lateral_buffer; - if (use_buffer) { - buffer = ros_parameters.hysteresis_buffer_distance; - // lateral_buffer = 0.5; - } else { - buffer = 0.0; - // lateral_buffer = 0.0; + if (route_handler->getPullOutStartLane( + route_handler->getShoulderLanelets(), current_pose, planner_data->parameters.vehicle_width, + &shoulder_lane)) { + shoulder_lanes = route_handler->getShoulderLaneletSequence( + shoulder_lane, current_pose, pull_out_lane_length, pull_out_lane_length); } - // find obstacle in shoulder lanes - const auto shoulder_lane_object_indices = - util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes); - - // Collision check for objects in shoulder lane - if (use_dynamic_object) { - for (const auto & i : shoulder_lane_object_indices) { - const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - - bool is_object_in_shoulder = false; - if (ros_parameters.use_predicted_path_outside_lanelet) { - is_object_in_shoulder = true; - } else { - for (const auto & llt : shoulder_lanes) { - if (lanelet::utils::isInLanelet( - obj.kinematics.initial_pose_with_covariance.pose, llt, 0.1)) { - is_object_in_shoulder = true; - } - } - } - // TODO(sugahara) static object judge - if (is_object_in_shoulder) { - const double distance = util::getDistanceBetweenPredictedPathAndObjectPolygon( - obj, path, local_vehicle_footprint, 1, road_lanes); - - double thresh = min_thresh + buffer; - if (distance < thresh) { - return false; - } - } else { - const double distance = util::getDistanceBetweenPredictedPathAndObjectPolygon( - obj, path, local_vehicle_footprint, 1, road_lanes); - double thresh = min_thresh + buffer; - if (distance < thresh) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "object is not in shoulder but close to the path."); - // return false; - } - } - } - } - - return true; -} - -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) -{ - tf2::Transform tf_map2ego, tf_map2obj; - Pose obj_from_ego; - tf2::fromMsg(ego_pose, tf_map2ego); - tf2::fromMsg(obj_pose, tf_map2obj); - tf2::toMsg(tf_map2ego.inverse() * tf_map2obj, obj_from_ego); - - return obj_from_ego.position.x > 0; + return shoulder_lanes; } } // namespace behavior_path_planner::pull_out_utils diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 4be0833db04d9..91f127a536e2c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -85,6 +85,28 @@ BehaviorModuleOutput PullOverModule::run() return plan(); } +ParallelParkingParameters PullOverModule::getGeometricPullOverParameters() const +{ + ParallelParkingParameters params; + + params.th_arrived_distance = parameters_.th_arrived_distance; + params.th_stopped_velocity = parameters_.th_stopped_velocity; + params.after_forward_parking_straight_distance = + parameters_.after_forward_parking_straight_distance; + params.after_backward_parking_straight_distance = + parameters_.after_backward_parking_straight_distance; + params.forward_parking_velocity = parameters_.forward_parking_velocity; + params.backward_parking_velocity = parameters_.backward_parking_velocity; + params.forward_parking_lane_departure_margin = parameters_.forward_parking_lane_departure_margin; + params.backward_parking_lane_departure_margin = + parameters_.backward_parking_lane_departure_margin; + params.arc_path_interval = parameters_.arc_path_interval; + params.maximum_deceleration = parameters_.maximum_deceleration; + params.max_steer_angle = parameters_.pull_over_max_steer_angle; + + return params; +} + void PullOverModule::onEntry() { RCLCPP_DEBUG(getLogger(), "PULL_OVER onEntry"); @@ -108,16 +130,7 @@ void PullOverModule::onEntry() last_received_time_.get() == nullptr || *last_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status - parallel_parking_planner_.clear(); - parallel_parking_parameters_ = ParallelParkingParameters{ - parameters_.th_arrived_distance_m, - parameters_.th_stopped_velocity_mps, - parameters_.after_forward_parking_straight_distance, - parameters_.after_backward_parking_straight_distance, - parameters_.forward_parking_velocity, - parameters_.backward_parking_velocity, - parameters_.arc_path_interval, - parameters_.min_acc}; + parallel_parking_parameters_ = getGeometricPullOverParameters(); resetStatus(); } @@ -239,7 +252,7 @@ void PullOverModule::researchGoal() const Pose goal_pose_map_coords = global2local(occupancy_grid_map_.getMap(), goal_pose); Pose start_pose = calcOffsetPose(goal_pose, dx, 0, 0); // Search non collision areas around the goal - while (true) { + while (rclcpp::ok()) { bool is_last_search = (dx >= parameters_.forward_goal_search_length); Pose search_pose = calcOffsetPose(goal_pose_map_coords, dx, 0, 0); bool is_collided = occupancy_grid_map_.detectCollision( @@ -304,7 +317,7 @@ BT::NodeStatus PullOverModule::updateState() } bool PullOverModule::isLongEnoughToParkingStart( - const PathWithLaneId path, const Pose parking_start_pose) const + const PathWithLaneId & path, const Pose & parking_start_pose) const { const auto dist_to_parking_start_pose = calcSignedArcLength( path.points, planner_data_->self_pose->pose, parking_start_pose.position, @@ -313,7 +326,7 @@ bool PullOverModule::isLongEnoughToParkingStart( const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double current_to_stop_distance = - std::pow(current_vel, 2) / std::abs(parameters_.min_acc) / 2; + std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; // once stopped, it cannot start again if start_pose is close. // so need enough distance to restart @@ -350,9 +363,10 @@ bool PullOverModule::planWithEfficientPath() if (parameters_.enable_arc_forward_parking) { for (const auto goal_candidate : goal_candidates_) { modified_goal_pose_ = goal_candidate.goal_pose; - parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_); + parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); if ( - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, true) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -371,9 +385,10 @@ bool PullOverModule::planWithEfficientPath() if (parameters_.enable_arc_backward_parking) { for (const auto goal_candidate : goal_candidates_) { modified_goal_pose_ = goal_candidate.goal_pose; - parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_); + parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); if ( - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, false) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -408,11 +423,12 @@ bool PullOverModule::planWithCloseGoal() return true; } - parallel_parking_planner_.setParams(planner_data_, parallel_parking_parameters_); + parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); // Generate arc forward path. if ( parameters_.enable_arc_forward_parking && - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, true) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -428,7 +444,8 @@ bool PullOverModule::planWithCloseGoal() // Generate arc backward path. if ( parameters_.enable_arc_backward_parking && - parallel_parking_planner_.plan(modified_goal_pose_, status_.lanes, false) && + parallel_parking_planner_.planPullOver( + modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && @@ -553,8 +570,18 @@ BehaviorModuleOutput PullOverModule::plan() } BehaviorModuleOutput output; - output.path = status_.is_safe ? std::make_shared(status_.path) - : std::make_shared(getStopPath()); + + // safe: use pull over path + if (status_.is_safe) { + output.path = std::make_shared(status_.path); + } else if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + // safe -> not_safe or no prev_stop_path: generate new stop_path + output.path = std::make_shared(generateStopPath()); + status_.prev_stop_path = output.path; + } else { // not_safe -> not_safe: use previous stop path + output.path = status_.prev_stop_path; + } + status_.prev_is_safe = status_.is_safe; // set hazard and turn signal if (status_.has_decided_path) { @@ -679,7 +706,7 @@ PathWithLaneId PullOverModule::getReferencePath() const // stop pose is behind current pose if (s_forward < s_backward) { - return getStopPath(); + return generateStopPath(); } PathWithLaneId reference_path = @@ -704,45 +731,43 @@ PathWithLaneId PullOverModule::getReferencePath() const return reference_path; } -PathWithLaneId PullOverModule::getStopPath() const +PathWithLaneId PullOverModule::generateStopPath() const { - PathWithLaneId reference_path; - const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; const auto common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - const double current_to_stop_distance = - std::pow(current_vel, 2) / std::abs(parameters_.min_acc) / 2; - const auto arclength_current_pose = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; - - reference_path = util::getCenterLinePath( + auto stop_path = util::getCenterLinePath( *route_handler, status_.current_lanes, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length, common_parameters); - for (auto & point : reference_path.points) { - const auto arclength = + const double current_to_stop_distance = + std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + const double arclength_current_pose = + lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; + + for (auto & point : stop_path.points) { + const double arclength = lanelet::utils::getArcCoordinates(status_.current_lanes, point.point.pose).length; - const auto arclength_current_to_point = arclength - arclength_current_pose; - if (0 < arclength_current_to_point) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast(std::max( - 0.0, current_vel * (current_to_stop_distance - arclength_current_to_point) / - current_to_stop_distance))); + const double arclength_current_to_point = arclength - arclength_current_pose; + if (0.0 < arclength_current_to_point) { + point.point.longitudinal_velocity_mps = std::clamp( + static_cast( + current_vel * (current_to_stop_distance - arclength_current_to_point) / + current_to_stop_distance), + 0.0f, point.point.longitudinal_velocity_mps); } else { point.point.longitudinal_velocity_mps = std::min(point.point.longitudinal_velocity_mps, static_cast(current_vel)); } } - reference_path.drivable_area = util::generateDrivableArea( - reference_path, status_.current_lanes, common_parameters.drivable_area_resolution, + stop_path.drivable_area = util::generateDrivableArea( + stop_path, status_.current_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length, planner_data_); - return reference_path; + return stop_path; } lanelet::ConstLanelets PullOverModule::getPullOverLanes() const @@ -846,7 +871,7 @@ double PullOverModule::calcMinimumShiftPathDistance() const } bool PullOverModule::isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose goal_pose, const double buffer) const + const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer) const { const auto current_pose = planner_data_->self_pose->pose; const double distance_to_goal = @@ -859,10 +884,10 @@ bool PullOverModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); // Delete old data in buffer - while (true) { + while (rclcpp::ok()) { const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - rclcpp::Time(odometry_buffer_.front()->header.stamp); - if (time_diff.seconds() < parameters_.th_stopped_time_sec) { + if (time_diff.seconds() < parameters_.th_stopped_time) { break; } odometry_buffer_.pop_front(); @@ -870,7 +895,7 @@ bool PullOverModule::isStopped() bool is_stopped = true; for (const auto & odometry : odometry_buffer_) { const double ego_vel = util::l2Norm(odometry->twist.twist.linear); - if (ego_vel > parameters_.th_stopped_velocity_mps) { + if (ego_vel > parameters_.th_stopped_velocity) { is_stopped = false; break; } @@ -883,7 +908,7 @@ bool PullOverModule::hasFinishedCurrentPath() const auto current_path_end = status_.path.points.back(); const auto self_pose = planner_data_->self_pose->pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < - parameters_.th_arrived_distance_m; + parameters_.th_arrived_distance; return is_near_target && isStopped(); } @@ -893,7 +918,7 @@ bool PullOverModule::hasFinishedPullOver() // check ego car is close enough to goal pose const auto current_pose = planner_data_->self_pose->pose; const bool car_is_on_goal = - calcDistance2d(current_pose, modified_goal_pose_) < parameters_.th_arrived_distance_m; + calcDistance2d(current_pose, modified_goal_pose_) < parameters_.th_arrived_distance; return car_is_on_goal && isStopped(); } @@ -966,7 +991,7 @@ bool PullOverModule::isArcPath() const } Marker PullOverModule::createParkingAreaMarker( - const Pose start_pose, const Pose end_pose, const int32_t id) + const Pose & start_pose, const Pose & end_pose, const int32_t id) { 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 diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index 966dba5d20a28..f13f753164256 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -371,7 +371,7 @@ bool isPullOverPathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const double vehicle_width, + const size_t current_seg_idx, const Twist & current_twist, const double vehicle_width, const PullOverParameters & ros_parameters, const bool use_buffer, const double acceleration) { if (path.points.empty()) { @@ -414,17 +414,18 @@ bool isPullOverPathSafe( // find obstacle in pull_over target lanes // retrieve lanes that are merging target lanes as well const auto target_lane_object_indices = - util::filterObjectsByLanelets(*dynamic_objects, target_lanes); + util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); // find objects in current lane - const auto current_lane_object_indices_lanelet = util::filterObjectsByLanelets( + const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); - const auto current_lane_object_indices = util::filterObjectsByPath( + const auto current_lane_object_indices = util::filterObjectsIndicesByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, vehicle_width / 2 + lateral_buffer); const auto & vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration); + path, current_twist, current_pose, current_seg_idx, target_lane_check_end_time, time_resolution, + acceleration); // Collision check for objects in current lane for (const auto & i : current_lane_object_indices) { diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp index cef1669005d74..33c7ba62cb0a1 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp @@ -45,6 +45,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() scene_module_->onEntry(); module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); + module_status_->is_execution_ready = scene_module_->isExecutionReady(); const bool is_lane_following = scene_module_->name() == "LaneFollowing"; @@ -59,6 +60,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() RCLCPP_ERROR_STREAM(scene_module_->getLogger(), "setOutput() failed : " << res.error()); } module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); + module_status_->is_execution_ready = scene_module_->isExecutionReady(); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( scene_module_->getLogger(), "behavior module has failed with exception: " << e.what()); @@ -81,6 +83,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() // for data output module_status_->status = current_status; module_status_->is_waiting_approval = scene_module_->isWaitingApproval(); + module_status_->is_execution_ready = scene_module_->isExecutionReady(); } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( scene_module_->getLogger(), "behavior module has failed with exception: " << e.what()); 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 59e57b1c019dc..39c6145168931 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 @@ -175,7 +175,8 @@ void SideShiftModule::updateData() current_lanelets_ = route_handler->getLaneletSequence( current_lane, reference_pose.pose, p.backward_path_length, p.forward_path_length); - path_shifter_.removeBehindShiftPointAndSetBaseOffset(planner_data_->self_pose->pose.position); + const size_t nearest_idx = findEgoIndex(path_shifter_.getReferencePath().points); + path_shifter_.removeBehindShiftPointAndSetBaseOffset(planner_data_->self_pose->pose, nearest_idx); } bool SideShiftModule::addShiftPoint() @@ -362,7 +363,6 @@ ShiftPoint SideShiftModule::calcShiftPoint() const { const auto & p = parameters_; const auto ego_speed = std::abs(planner_data_->self_odometry->twist.twist.linear.x); - const auto ego_pose = planner_data_->self_pose->pose; const double dist_to_start = std::max(p.min_distance_to_start_shifting, ego_speed * p.time_to_start_shifting); @@ -379,11 +379,12 @@ ShiftPoint SideShiftModule::calcShiftPoint() const return dist_to_end; }(); + const size_t nearest_idx = findEgoIndex(reference_path_->points); ShiftPoint shift_point; shift_point.length = lateral_offset_; - shift_point.start_idx = util::getIdxByArclength(*reference_path_, ego_pose, dist_to_start); + shift_point.start_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_start); shift_point.start = reference_path_->points.at(shift_point.start_idx).point.pose; - shift_point.end_idx = util::getIdxByArclength(*reference_path_, ego_pose, dist_to_end); + shift_point.end_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_end); shift_point.end = reference_path_->points.at(shift_point.end_idx).point.pose; return shift_point; diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index e6325dc2e042b..ec0583215908c 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -39,11 +39,14 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::util::convertToGeometryPoseArray; +using behavior_path_planner::util::removeOverlappingPoints; +using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Transform; using geometry_msgs::msg::TransformStamped; +using lanelet::utils::getArcCoordinates; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::deg2rad; @@ -60,6 +63,15 @@ void GeometricParallelParking::incrementPathIndex() current_path_idx_ = std::min(current_path_idx_ + 1, paths_.size() - 1); } +PathWithLaneId GeometricParallelParking::getPathByIdx(size_t const idx) const +{ + if (paths_.empty() || paths_.size() <= idx) { + return PathWithLaneId{}; + } + + return paths_.at(idx); +} + PathWithLaneId GeometricParallelParking::getCurrentPath() const { return paths_.at(current_path_idx_); @@ -68,203 +80,385 @@ PathWithLaneId GeometricParallelParking::getCurrentPath() const PathWithLaneId GeometricParallelParking::getFullPath() const { PathWithLaneId path{}; - for (const auto & p : paths_) { - path.points.insert(path.points.end(), p.points.begin(), p.points.end()); + for (const auto & partial_path : paths_) { + path.points.insert(path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - return path; + + return removeOverlappingPoints(path); } PathWithLaneId GeometricParallelParking::getArcPath() const { PathWithLaneId path{}; - for (size_t i = 1; i < paths_.size(); i++) { - const auto p = paths_.at(i); - path.points.insert(path.points.end(), p.points.begin(), p.points.end()); + for (const auto & arc_path : arc_paths_) { + path.points.insert(path.points.end(), arc_path.points.begin(), arc_path.points.end()); } return path; } -void GeometricParallelParking::clear() +bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } + +bool GeometricParallelParking::isEnoughDistanceToStart(const Pose & start_pose) const +{ + const Pose current_pose = planner_data_->self_pose->pose; + const Pose current_to_start = + inverseTransformPose(start_pose, current_pose); // todo: arc length is better + + // not enough to stop with max deceleration + const double current_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); + const double stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + if (current_to_start.position.x < stop_distance) { + return false; + } + + // not enough to restart from stopped + constexpr double min_restart_distance = 3.0; + if ( + current_vel < parameters_.th_stopped_velocity && + current_to_start.position.x > parameters_.th_arrived_distance && + current_to_start.position.x < min_restart_distance) { + return false; + } + + return true; +} + +void GeometricParallelParking::setVelocityToArcPaths( + std::vector & arc_paths, const double velocity) +{ + for (auto & path : arc_paths) { + for (size_t i = 0; i < path.points.size(); i++) { + if (i == path.points.size() - 1) { + // stop point at the end of the path + path.points.at(i).point.longitudinal_velocity_mps = 0.0; + } else { + path.points.at(i).point.longitudinal_velocity_mps = velocity; + } + } + } +} + +std::vector GeometricParallelParking::generatePullOverPaths( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const double end_pose_offset, const double velocity) +{ + if (!isEnoughDistanceToStart(start_pose)) { + return std::vector{}; + } + const double lane_departure_margin = is_forward + ? parameters_.forward_parking_lane_departure_margin + : parameters_.backward_parking_lane_departure_margin; + auto arc_paths = planOneTrial( + start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, + lane_departure_margin); + if (arc_paths.empty()) { + return std::vector{}; + } + arc_paths_ = arc_paths; + + // set parking velocity and stop velocity at the end of the path + setVelocityToArcPaths(arc_paths, velocity); + + // straight path from current to parking start + const auto straight_path = generateStraightPath(start_pose); + + // combine straight_path -> arc_path*2 + auto paths = arc_paths; + paths.insert(paths.begin(), straight_path); + + return paths; +} + +void GeometricParallelParking::clearPaths() { current_path_idx_ = 0; + arc_paths_.clear(); + path_pose_array_.poses.clear(); paths_.clear(); } -bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } - -bool GeometricParallelParking::plan( - const Pose goal_pose, const lanelet::ConstLanelets lanes, const bool is_forward) +bool GeometricParallelParking::planPullOver( + const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward) { - const auto common_params = planner_data_->parameters; - // plan path only when parking has not started - if (!isParking()) { - if (is_forward) { - // When turning forward to the right, the front left goes out, - // so reduce the steer angle at that time for seach no lane departure path. - for (double steer = max_steer_rad_; steer > 0.05; steer -= 0.1) { - const double R_E_r = common_params.wheel_base / std::tan(steer); - if (planOneTraial(goal_pose, 0, R_E_r, lanes, is_forward)) return true; + clearPaths(); + + const auto & common_params = planner_data_->parameters; + const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance + : parameters_.after_backward_parking_straight_distance; + const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); + + // not plan after parking has started + if (isParking()) { + return false; + } + + if (is_forward) { + // When turning forward to the right, the front left goes out, + // so reduce the steer angle at that time for seach no lane departure path. + constexpr double start_pose_offset = 0.0; + constexpr double min_steer_rad = 0.05; + constexpr double steer_interval = 0.1; + for (double steer = parameters_.max_steer_angle; steer > min_steer_rad; + steer -= steer_interval) { + const double R_E_r = common_params.wheel_base / std::tan(steer); + const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_r, is_forward); + if (!start_pose) { + continue; + } + + const auto paths = generatePullOverPaths( + *start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, + parameters_.forward_parking_velocity); + if (!paths.empty()) { + paths_ = paths; + return true; + } + } + } else { // backward + // When turning backward to the left, the front right goes out, + // so make the parking start point in front for seach no lane departure path + // (same to reducing the steer angle) + constexpr double max_offset = 5.0; + constexpr double offset_interval = 1.0; + for (double start_pose_offset = 0; start_pose_offset < max_offset; + start_pose_offset += offset_interval) { + const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_min_, is_forward); + if (!start_pose) { + continue; + } + + const auto paths = generatePullOverPaths( + *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, end_pose_offset, + parameters_.backward_parking_velocity); + if (!paths.empty()) { + paths_ = paths; + return true; } - } else { - // When turning backward to the left, the front right goes out, - // so make the parking start point in front for seach no lane departure path - // (same to reducing the steer angle) - for (double dx = 0; dx < 5; dx += 1.0) { - if (planOneTraial(goal_pose, dx, R_E_min_, lanes, is_forward)) return true; + } + } + + return false; +} + +bool GeometricParallelParking::planPullOut( + const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes) +{ + clearPaths(); + + constexpr bool is_forward = false; // parking backward means departing forward + constexpr double start_pose_offset = 0.0; // start_pose is current_pose + constexpr double max_offset = 10.0; + constexpr double offset_interval = 1.0; + + for (double end_pose_offset = 0; end_pose_offset < max_offset; + end_pose_offset += offset_interval) { + // departing end pose which is the second arc path end + const auto end_pose = calcStartPose(start_pose, end_pose_offset, R_E_min_, is_forward); + if (!end_pose) { + continue; + } + + // plan reverse path of parking. end_pose <-> start_pose + auto arc_paths = planOneTrial( + *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, start_pose_offset, + parameters_.departing_lane_departure_margin); + if (arc_paths.empty()) { + // not found path + continue; + } + + // reverse to turn_right -> turn_left + std::reverse(arc_paths.begin(), arc_paths.end()); + + // reverse path points order + for (auto & path : arc_paths) { + std::reverse(path.points.begin(), path.points.end()); + } + + // reverse lane_ids. shoulder, lane order + for (auto & path : arc_paths) { + for (auto & p : path.points) { + std::reverse(p.lane_ids.begin(), p.lane_ids.end()); } } + + arc_paths_ = arc_paths; + + // get road center line path from departing end to goal, and combine after the second arc path + PathWithLaneId road_center_line_path; + { + const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0; // need buffer? + const double s_end = getArcCoordinates(road_lanes, goal_pose).length; + road_center_line_path = + planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + } + auto paths = arc_paths; + paths.back().points.insert( + paths.back().points.end(), road_center_line_path.points.begin(), + road_center_line_path.points.end()); + removeOverlappingPoints(paths.back()); + + // set departing velocity and stop velocity at the end of the path + setVelocityToArcPaths(paths, parameters_.departing_velocity); + paths_ = paths; + + return true; } return false; } -Pose GeometricParallelParking::calcStartPose( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, const bool is_forward) +boost::optional GeometricParallelParking::calcStartPose( + const Pose & goal_pose, const double start_pose_offset, const double R_E_r, const bool is_forward) { // Not use shoulder lanes. const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanes, goal_pose); // todo - // When forwarding, the turning radius of the right and left are the same. + // When forwarding, the turning radius of the right and left will be the same. // But the left turn should also have a minimum turning radius. - float dx = 2 * std::sqrt(std::pow(R_E_r, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_r, 2)); - dx = is_forward ? -dx : dx; - Pose start_pose = calcOffsetPose(goal_pose, dx + start_pose_offset, -arc_coordinates.distance, 0); + // see https://www.sciencedirect.com/science/article/pii/S1474667016436852 for the dx detail + const double squared_distance_to_arc_connect = + std::pow(R_E_r, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_r, 2); + if (squared_distance_to_arc_connect < 0) { + // may be current_pose is behind the lane + return boost::none; + } + const double dx_sign = is_forward ? -1 : 1; + const double dx = 2 * std::sqrt(squared_distance_to_arc_connect) * dx_sign; + const Pose start_pose = + calcOffsetPose(goal_pose, dx + start_pose_offset, -arc_coordinates.distance, 0); return start_pose; } -void GeometricParallelParking::generateStraightPath(const Pose start_pose) +PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start_pose) { - // get stright path before parking. + // get straight path before parking. const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); const auto start_arc_position = lanelet::utils::getArcCoordinates(current_lanes, start_pose); const Pose current_pose = planner_data_->self_pose->pose; - const auto crrent_arc_position = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + const auto current_arc_position = lanelet::utils::getArcCoordinates(current_lanes, current_pose); auto path = planner_data_->route_handler->getCenterLinePath( - current_lanes, crrent_arc_position.length, start_arc_position.length, true); + current_lanes, current_arc_position.length, start_arc_position.length, true); path.header = planner_data_->route_handler->getRouteHeader(); - const auto common_params = planner_data_->parameters; path.drivable_area = util::generateDrivableArea( - path, current_lanes, common_params.drivable_area_resolution, common_params.vehicle_length, - planner_data_); + path, current_lanes, planner_data_->parameters.drivable_area_resolution, + planner_data_->parameters.vehicle_length, planner_data_); path.points.back().point.longitudinal_velocity_mps = 0; - paths_.push_back(path); + return path; } -bool GeometricParallelParking::planOneTraial( - const Pose goal_pose, const double start_pose_offset, const double R_E_r, - const lanelet::ConstLanelets lanes, const bool is_forward) +std::vector GeometricParallelParking::planOneTrial( + const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, + const bool is_forward, const double end_pose_offset, const double lane_departure_margin) { - path_pose_array_.poses.clear(); - paths_.clear(); - - const double after_parking_straight_distance = - is_forward ? -parameters_.after_forward_parking_straight_distance - : parameters_.after_backward_parking_straight_distance; - const Pose arc_end_pose = calcOffsetPose(goal_pose, after_parking_straight_distance, 0, 0); - const Pose start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_r, is_forward); - const Pose current_pose = planner_data_->self_pose->pose; - const Pose current_to_start = inverseTransformPose(start_pose, current_pose); - - const double current_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); - const double stop_distance = std::pow(current_vel, 2) / std::abs(parameters_.min_acc) / 2; - if (current_to_start.position.x < stop_distance) { - return false; - } - // not enogh to restart from stopped - if ( - current_vel < parameters_.th_stopped_velocity_mps && - current_to_start.position.x > parameters_.th_arrived_distance_m && - current_to_start.position.x < 3.0) { - return false; - } - - const float self_yaw = tf2::getYaw(start_pose.orientation); - const float goal_yaw = tf2::getYaw(arc_end_pose.orientation); - const float psi = normalizeRadian(self_yaw - goal_yaw); const auto common_params = planner_data_->parameters; - Pose Cr = calcOffsetPose(arc_end_pose, 0, -R_E_r, 0); - const float d_Cr_Einit = calcDistance2d(Cr, start_pose); + const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); + const double self_yaw = tf2::getYaw(start_pose.orientation); + const double goal_yaw = tf2::getYaw(arc_end_pose.orientation); + const double psi = normalizeRadian(self_yaw - goal_yaw); - geometry_msgs::msg::Point Cr_goalcoords = inverseTransformPoint(Cr.position, arc_end_pose); - geometry_msgs::msg::Point self_point_goalcoords = - inverseTransformPoint(start_pose.position, arc_end_pose); + const Pose Cr = calcOffsetPose(arc_end_pose, 0, -R_E_r, 0); + const double d_Cr_Einit = calcDistance2d(Cr, start_pose); - const float alpha = + const Point Cr_goalcoords = inverseTransformPoint(Cr.position, arc_end_pose); + const Point self_point_goalcoords = inverseTransformPoint(start_pose.position, arc_end_pose); + + const double alpha = M_PI_2 - psi + std::asin((self_point_goalcoords.y - Cr_goalcoords.y) / d_Cr_Einit); - const float R_E_l = + const double R_E_l = (std::pow(d_Cr_Einit, 2) - std::pow(R_E_r, 2)) / (2 * (R_E_r + d_Cr_Einit * std::cos(alpha))); if (R_E_l <= 0) { - return false; + return std::vector{}; } - // If start_pose is prallel to goal_pose, we can know lateral deviation of eges of vehicle, + // combine road and shoulder lanes + lanelet::ConstLanelets lanes = road_lanes; + lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + + // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle, // and detect lane departure. - // Check left bound - if (is_forward) { - const float R_front_left = + if (is_forward) { // Check left bound + const double R_front_left = std::hypot(R_E_r + common_params.vehicle_width / 2, common_params.base_link2front); - const double distance_to_left_bound = util::getDistanceToShoulderBoundary(lanes, arc_end_pose); - const float left_deviation = R_front_left - R_E_r; - if (std::abs(distance_to_left_bound) < left_deviation) { - return false; + const double distance_to_left_bound = + util::getDistanceToShoulderBoundary(shoulder_lanes, arc_end_pose); + const double left_deviation = R_front_left - R_E_r; + if (std::abs(distance_to_left_bound) - left_deviation < lane_departure_margin) { + return std::vector{}; } } else { // Check right bound - const float R_front_right = + const double R_front_right = std::hypot(R_E_l + common_params.vehicle_width / 2, common_params.base_link2front); - const float right_deviation = R_front_right - R_E_l; + const double right_deviation = R_front_right - R_E_l; const double distance_to_right_bound = util::getDistanceToRightBoundary(lanes, start_pose); - if (distance_to_right_bound < right_deviation) { - return false; + if (distance_to_right_bound - right_deviation < lane_departure_margin) { + return std::vector{}; } } - generateStraightPath(start_pose); - // Generate arc path(left turn -> right turn) - Pose Cl = calcOffsetPose(start_pose, 0, R_E_l, 0); - float theta_l = std::acos( + const Pose Cl = calcOffsetPose(start_pose, 0, R_E_l, 0); + double theta_l = std::acos( (std::pow(R_E_l, 2) + std::pow(R_E_l + R_E_r, 2) - std::pow(d_Cr_Einit, 2)) / (2 * R_E_l * (R_E_l + R_E_r))); theta_l = is_forward ? theta_l : -theta_l; + // get closest lanes + lanelet::Lanelet closest_road_lanelet; + lanelet::utils::query::getClosestLanelet(road_lanes, goal_pose, &closest_road_lanelet); + lanelet::Lanelet closest_shoulder_lanelet; + lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lanelet); + PathWithLaneId path_turn_left = generateArcPath(Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), is_forward, is_forward); - - PathWithLaneId path_turn_right = generateArcPath( - Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward); - // Need to add straight path to last right_turning for parking in parallel - PathPointWithLaneId straight_point{}; - lanelet::ConstLanelet goal_lane; - lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lane); - straight_point.point.pose = goal_pose; - // Use z of previous point because z of goal is 0. - // https://github.com/autowarefoundation/autoware.universe/issues/711 - straight_point.point.pose.position.z = path_turn_right.points.back().point.pose.position.z; - straight_point.point.longitudinal_velocity_mps = 0.0; - straight_point.lane_ids.push_back(goal_lane.id()); - path_turn_right.points.push_back(straight_point); - + for (auto & p : path_turn_left.points) { + p.lane_ids.push_back(closest_road_lanelet.id()); + p.lane_ids.push_back(closest_shoulder_lanelet.id()); + } path_turn_left.header = planner_data_->route_handler->getRouteHeader(); path_turn_left.drivable_area = util::generateDrivableArea( path_turn_left, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_); - paths_.push_back(path_turn_left); + PathWithLaneId path_turn_right = generateArcPath( + Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward); + for (auto & p : path_turn_right.points) { + p.lane_ids.push_back(closest_road_lanelet.id()); + p.lane_ids.push_back(closest_shoulder_lanelet.id()); + } path_turn_right.header = planner_data_->route_handler->getRouteHeader(); path_turn_right.drivable_area = util::generateDrivableArea( path_turn_right, lanes, common_params.drivable_area_resolution, common_params.vehicle_length, planner_data_); + + // Need to add straight path to last right_turning for parking in parallel + if (std::abs(end_pose_offset) > 0) { + PathPointWithLaneId straight_point{}; + lanelet::ConstLanelet goal_lane; + lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane); + straight_point.point.pose = goal_pose; + straight_point.lane_ids.push_back(goal_lane.id()); + path_turn_right.points.push_back(straight_point); + } + + // generate arc path vector + paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // debug Cr_.pose = Cr; Cr_.header = planner_data_->route_handler->getRouteHeader(); Cl_.pose = Cl; @@ -276,55 +470,50 @@ bool GeometricParallelParking::planOneTraial( path_pose_array_ = convertToGeometryPoseArray(getFullPath()); path_pose_array_.header = planner_data_->route_handler->getRouteHeader(); - return true; + return paths_; } PathWithLaneId GeometricParallelParking::generateArcPath( - const Pose & center, const float radius, const float start_yaw, float end_yaw, + const Pose & center, const double radius, const double start_yaw, double end_yaw, const bool is_left_turn, // is_left_turn means clockwise around center. const bool is_forward) { PathWithLaneId path; - - const float velocity = - is_forward ? parameters_.forward_parking_velocity : parameters_.backward_parking_velocity; - const float yaw_interval = parameters_.arc_path_interval / radius; - float yaw = start_yaw; + const double yaw_interval = parameters_.arc_path_interval / radius; + double yaw = start_yaw; if (is_left_turn) { if (end_yaw < start_yaw) end_yaw += M_PI_2; while (yaw < end_yaw) { - PathPointWithLaneId p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); - p.point.longitudinal_velocity_mps = velocity; + const auto p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); path.points.push_back(p); yaw += yaw_interval; } } else { // right_turn if (end_yaw > start_yaw) end_yaw -= M_PI_2; while (yaw > end_yaw) { - PathPointWithLaneId p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); - p.point.longitudinal_velocity_mps = velocity; + const auto p = generateArcPathPoint(center, radius, yaw, is_left_turn, is_forward); path.points.push_back(p); yaw -= yaw_interval; } } - PathPointWithLaneId p = generateArcPathPoint(center, radius, end_yaw, is_left_turn, is_forward); - // If last parking turn, going straight is needed for parking in parallel. - const bool is_final_turn = (is_left_turn && !is_forward) || (!is_left_turn && is_forward); - p.point.longitudinal_velocity_mps = is_final_turn ? velocity : 0; + // insert the last point exactly + const auto p = generateArcPathPoint(center, radius, end_yaw, is_left_turn, is_forward); path.points.push_back(p); - return path; + return removeOverlappingPoints(path); } PathPointWithLaneId GeometricParallelParking::generateArcPathPoint( - const Pose & center, const float radius, const float yaw, const bool is_left_turn, + const Pose & center, const double radius, const double yaw, const bool is_left_turn, const bool is_forward) { + // get pose in center_pose coords Pose pose_centercoords; pose_centercoords.position.x = radius * std::cos(yaw); pose_centercoords.position.y = radius * std::sin(yaw); + // set orientation tf2::Quaternion quat; if ((is_left_turn && !is_forward) || (!is_left_turn && is_forward)) { quat.setRPY(0, 0, normalizeRadian(yaw - M_PI_2)); @@ -333,38 +522,23 @@ PathPointWithLaneId GeometricParallelParking::generateArcPathPoint( } pose_centercoords.orientation = tf2::toMsg(quat); + // get pose in map coords PathPointWithLaneId p{}; p.point.pose = transformPose(pose_centercoords, center); - lanelet::ConstLanelet current_lane; - planner_data_->route_handler->getClosestLaneletWithinRoute(p.point.pose, ¤t_lane); - - // Use z of lanelet closest point because z of goal is 0. - // https://github.com/autowarefoundation/autoware.universe/issues/711 - double min_distance = std::numeric_limits::max(); - for (const auto & pt : current_lane.centerline3d()) { - const double distance = - calcDistance2d(p.point.pose, lanelet::utils::conversion::toGeomMsgPt(pt)); - if (distance < min_distance) { - min_distance = distance; - p.point.pose.position.z = pt.z(); - } - } - - p.lane_ids.push_back(current_lane.id()); return p; } -void GeometricParallelParking::setParams( - const std::shared_ptr & planner_data, ParallelParkingParameters parameters) +void GeometricParallelParking::setData( + const std::shared_ptr & planner_data, + const ParallelParkingParameters & parameters) { planner_data_ = planner_data; parameters_ = parameters; auto common_params = planner_data_->parameters; - max_steer_rad_ = deg2rad(max_steer_deg_); - R_E_min_ = common_params.wheel_base / std::tan(max_steer_rad_); + R_E_min_ = common_params.wheel_base / std::tan(parameters_.max_steer_angle); R_Bl_min_ = std::hypot( R_E_min_ + common_params.wheel_tread / 2 + common_params.left_over_hang, common_params.wheel_base + common_params.front_overhang); diff --git a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp index 4047c4b2612fd..d89cf8ade1e9d 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp @@ -63,7 +63,7 @@ void PathShifter::setShiftPoints(const std::vector & points) bool PathShifter::generate( ShiftedPath * shifted_path, const bool offset_back, const SHIFT_TYPE type) { - RCLCPP_DEBUG_STREAM(logger_, "PathShifter::generate start!"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "PathShifter::generate start!"); // Guard if (reference_path_.points.empty()) { @@ -75,7 +75,8 @@ bool PathShifter::generate( shifted_path->shift_length.resize(reference_path_.points.size(), 0.0); if (shift_points_.empty()) { - RCLCPP_DEBUG_STREAM(logger_, "shift_points_ is empty. Return reference with base offset."); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, clock_, 3000, "shift_points_ is empty. Return reference with base offset."); shiftBaseLength(shifted_path, base_offset_); return true; } @@ -85,8 +86,8 @@ bool PathShifter::generate( for (const auto & shift_point : shift_points_) { int idx_gap = shift_point.end_idx - shift_point.start_idx; if (idx_gap <= 1) { - RCLCPP_WARN_STREAM( - logger_, + RCLCPP_WARN_STREAM_THROTTLE( + logger_, clock_, 3000, "shift start point and end point can't be adjoining " "Maybe shift length is too short?"); return false; @@ -96,15 +97,15 @@ bool PathShifter::generate( // Sort shift points since applyShift function only supports sorted points if (!sortShiftPointsAlongPath(reference_path_)) { - RCLCPP_ERROR_STREAM(logger_, "has duplicated points. Failed!"); + RCLCPP_ERROR_STREAM_THROTTLE(logger_, clock_, 3000, "has duplicated points. Failed!"); return false; } if (shift_points_.front().start_idx == 0) { // if offset is applied on front side, shifting from first point is no problem if (offset_back) { - RCLCPP_WARN_STREAM( - logger_, + RCLCPP_WARN_STREAM_THROTTLE( + logger_, clock_, 3000, "shift start point is at the edge of path. It could cause undesired result." " Maybe path is too short for backward?"); } @@ -118,8 +119,9 @@ bool PathShifter::generate( motion_utils::insertOrientation(shifted_path->path.points, is_driving_forward); // DEBUG - RCLCPP_DEBUG_STREAM( - logger_, "PathShifter::generate end. shift_points_.size = " << shift_points_.size()); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, clock_, 3000, + "PathShifter::generate end. shift_points_.size = " << shift_points_.size()); return true; } @@ -201,7 +203,7 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs query_distance.push_back(dist_from_start); } if (!query_distance.empty()) { - query_length = interpolation::slerp(base_distance, base_length, query_distance); + query_length = interpolation::spline(base_distance, base_length, query_distance); } // Apply shifting. @@ -248,74 +250,17 @@ std::vector PathShifter::calcLateralJerk() return lateral_jerk; } -bool PathShifter::calcShiftPointFromArcLength( - const PathWithLaneId & path, const Point & origin, double dist_to_start, double dist_to_end, - double shift_length, ShiftPoint * shift_point) -{ - if (dist_to_end < dist_to_start) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("path_shifter"), - "dist_to_end must be longer than dist_to_start. Failed to create shift point"); - return false; - } - - if (path.points.empty()) { - return false; - } - - const auto origin_idx = motion_utils::findNearestIndex(path.points, origin); - const auto arclength_from_origin = util::calcPathArcLengthArray(path, origin_idx); - - if (dist_to_end > arclength_from_origin.back()) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("path_shifter"), - "path is too short to calculate shift point: path length from origin = " - << arclength_from_origin.back() << ", desired dist_to_end = " << dist_to_end); - } - - bool is_start_found = false; - bool is_end_found = false; - const auto getPathPointFromOrigin = [&](size_t idx_from_origin) { - return path.points.at(idx_from_origin + origin_idx).point.pose; - }; - for (size_t i = 0; i < arclength_from_origin.size() - 1; ++i) { - if (!is_start_found && arclength_from_origin.at(i + 1) > dist_to_start) { - shift_point->start = getPathPointFromOrigin(i); - is_start_found = true; - } - if (!is_end_found && arclength_from_origin.at(i + 1) > dist_to_end) { - shift_point->end = getPathPointFromOrigin(i); - is_end_found = true; - break; - } - } - - if (!is_start_found) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("path_shifter"), - "Even start point has not found. You should stop the shifting. Failed to create shift point"); - return false; - } - - if (is_start_found && !is_end_found) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("path_shifter"), - "failed to find end point. Set end point to path.back()."); - shift_point->end = path.points.back().point.pose; - } - - shift_point->length = shift_length; - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("path_shifter"), - "Shift Point is generated from arclength: shift_length = %f", shift_length); - return true; -} - void PathShifter::updateShiftPointIndices() { for (auto & p : shift_points_) { - p.start_idx = motion_utils::findNearestIndex(reference_path_.points, p.start.position); - p.end_idx = motion_utils::findNearestIndex(reference_path_.points, p.end.position); + p.start_idx = motion_utils::findNearestIndex( + reference_path_.points, + p.start.position); // TODO(murooka) remove findNearestIndex for except + // lane_following to support u-turn & crossing path + p.end_idx = motion_utils::findNearestIndex( + reference_path_.points, + p.end.position); // TODO(murooka) remove findNearestIndex for except + // lane_following to support u-turn & crossing path } is_index_aligned_ = true; } @@ -339,7 +284,7 @@ bool PathShifter::checkShiftPointsAlignment(const ShiftPointArray & shift_points bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId & path) { if (shift_points_.empty()) { - RCLCPP_DEBUG_STREAM(logger_, "shift_points_ is empty. do nothing."); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "shift_points_ is empty. do nothing."); return true; } @@ -369,29 +314,30 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId // Debug for (const auto & p : unsorted_shift_points) { - RCLCPP_DEBUG_STREAM(logger_, "unsorted_shift_points: " << toStr(p)); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "unsorted_shift_points: " << toStr(p)); } for (const auto & i : sorted_indices) { - RCLCPP_DEBUG_STREAM(logger_, "sorted_indices i = " << i); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "sorted_indices i = " << i); } for (const auto & p : sorted_shift_points) { - RCLCPP_DEBUG_STREAM(logger_, "sorted_shift_points: in order: " << toStr(p)); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, clock_, 3000, "sorted_shift_points: in order: " << toStr(p)); } RCLCPP_DEBUG(logger_, "PathShifter::sortShiftPointsAlongPath end."); return true; } -void PathShifter::removeBehindShiftPointAndSetBaseOffset(const Point & base_point) +void PathShifter::removeBehindShiftPointAndSetBaseOffset( + [[maybe_unused]] const Pose & pose, const size_t nearest_idx) { - const auto base_idx = motion_utils::findNearestIndex(reference_path_.points, base_point); - - // If shift_point.end is behind the base_point, remove the shift_point and + // If shift_point.end is behind the ego_pose, remove the shift_point and // set its shift_length to the base_offset. ShiftPointArray new_shift_points; ShiftPointArray removed_shift_points; for (const auto & sp : shift_points_) { - (sp.end_idx > base_idx) ? new_shift_points.push_back(sp) : removed_shift_points.push_back(sp); + (sp.end_idx > nearest_idx) ? new_shift_points.push_back(sp) + : removed_shift_points.push_back(sp); } double new_base_offset = base_offset_; diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index b7a6ea72cf92a..40dbf83f31b0a 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -25,14 +25,16 @@ namespace behavior_path_planner { TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( - const PathWithLaneId & path, const Pose & current_pose, const RouteHandler & route_handler, - const TurnIndicatorsCommand & turn_signal_plan, const double plan_distance) const + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const RouteHandler & route_handler, const TurnIndicatorsCommand & turn_signal_plan, + const double plan_distance) const { auto turn_signal = turn_signal_plan; // If the distance to intersection is nearer than path change point, // use turn signal for turning at the intersection - const auto intersection_result = getIntersectionTurnSignal(path, current_pose, route_handler); + const auto intersection_result = + getIntersectionTurnSignal(path, current_pose, current_seg_idx, route_handler); const auto intersection_turn_signal = intersection_result.first; const auto intersection_distance = intersection_result.second; @@ -47,7 +49,8 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( } std::pair TurnSignalDecider::getIntersectionTurnSignal( - const PathWithLaneId & path, const Pose & current_pose, const RouteHandler & route_handler) const + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const RouteHandler & route_handler) const { TurnIndicatorsCommand turn_signal{}; turn_signal.command = TurnIndicatorsCommand::DISABLE; @@ -59,7 +62,8 @@ std::pair TurnSignalDecider::getIntersectionTurnS } // Get frenet coordinate of current_pose on path - const auto vehicle_pose_frenet = util::convertToFrenetCoordinate3d(path, current_pose.position); + const auto vehicle_pose_frenet = + util::convertToFrenetCoordinate3d(path, current_pose.position, current_seg_idx); // Get nearest intersection and decide turn signal double accumulated_distance = 0; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 87a4f31f0623f..1e6d796c0d953 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -18,6 +18,9 @@ #include #include +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" + #include #include #include @@ -58,6 +61,20 @@ double calcInterpolatedVelocity( namespace drivable_area_utils { +template +size_t findNearestSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ + const auto nearest_idx = + motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); + if (nearest_idx) { + return nearest_idx.get(); + } + + return motion_utils::findNearestSegmentIndex(points, pose.position); +} + double quantize(const double val, const double resolution) { return std::round(val / resolution) * resolution; @@ -95,11 +112,28 @@ bool sumLengthFromTwoPoints( return is_end; } +void fillYawFromXY(std::vector & points) +{ + if (points.size() < 2) { + return; + } + + for (size_t i = 0; i < points.size(); ++i) { + const size_t prev_idx = (i == points.size() - 1) ? i - 1 : i; + const size_t next_idx = (i == points.size() - 1) ? i : i + 1; + + const double yaw = tier4_autoware_utils::calcAzimuthAngle( + points.at(prev_idx).position, points.at(next_idx).position); + points.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + } +} + std::array getPathScope( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & current_pose, const double forward_lane_length, - const double backward_lane_length, const double lane_margin) + const double backward_lane_length, const double lane_margin, const double max_dist, + const double max_yaw) { // extract lanes from path_with_lane_id lanelet::ConstLanelets path_lanes = [&]() { @@ -159,19 +193,21 @@ std::array getPathScope( continue; } - std::vector points; - for (const auto & point : nearest_bound) { - geometry_msgs::msg::Point p; - p.x = point.x(); - p.y = point.y(); + std::vector points; + for (const auto & point : nearest_bound) { // calculate x and y + geometry_msgs::msg::Pose p; + p.position.x = point.x(); + p.position.y = point.y(); points.push_back(p); } + fillYawFromXY(points); // calculate yaw const size_t nearest_segment_idx = - motion_utils::findNearestSegmentIndex(points, current_pose.position); + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, current_pose, max_dist, max_yaw); // forward lanelet - const auto forward_offset_length = - motion_utils::calcSignedArcLength(points, current_pose.position, nearest_segment_idx); + const auto forward_offset_length = motion_utils::calcSignedArcLength( + points, current_pose.position, nearest_segment_idx, nearest_segment_idx); double sum_length = std::min(forward_offset_length, 0.0); size_t current_lane_idx = nearest_lane_idx; auto current_lane = path_lanes.at(current_lane_idx); @@ -218,8 +254,8 @@ std::array getPathScope( // backward lanelet current_point_idx = nearest_segment_idx + 1; - const auto backward_offset_length = - motion_utils::calcSignedArcLength(points, nearest_segment_idx + 1, current_pose.position); + const auto backward_offset_length = motion_utils::calcSignedArcLength( + points, nearest_segment_idx + 1, current_pose.position, nearest_segment_idx); sum_length = std::min(backward_offset_length, 0.0); current_lane_idx = nearest_lane_idx; current_lane = path_lanes.at(current_lane_idx); @@ -279,15 +315,16 @@ namespace behavior_path_planner::util using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; +using tf2::fromMsg; using tier4_autoware_utils::Point2d; -std::vector convertToPointArray(const PathWithLaneId & path) +std::vector convertToPoseArray(const PathWithLaneId & path) { - std::vector point_array; + std::vector pose_array; for (const auto & pt : path.points) { - point_array.push_back(pt.point.pose.position); + pose_array.push_back(pt.point.pose); } - return point_array; + return pose_array; } double l2Norm(const Vector3 vector) @@ -295,29 +332,6 @@ double l2Norm(const Vector3 vector) return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2)); } -FrenetCoordinate3d convertToFrenetCoordinate3d( - const std::vector & linestring, const Point & search_point_geom) -{ - FrenetCoordinate3d frenet_coordinate; - - const size_t nearest_segment_idx = - motion_utils::findNearestSegmentIndex(linestring, search_point_geom); - const double longitudinal_length = motion_utils::calcLongitudinalOffsetToSegment( - linestring, nearest_segment_idx, search_point_geom); - frenet_coordinate.length = - motion_utils::calcSignedArcLength(linestring, 0, nearest_segment_idx) + longitudinal_length; - frenet_coordinate.distance = motion_utils::calcLateralOffset(linestring, search_point_geom); - - return frenet_coordinate; -} - -FrenetCoordinate3d convertToFrenetCoordinate3d( - const PathWithLaneId & path, const Point & search_point_geom) -{ - const auto linestring = convertToPointArray(path); - return convertToFrenetCoordinate3d(linestring, search_point_geom); -} - std::vector convertToGeometryPointArray(const PathWithLaneId & path) { std::vector converted_path; @@ -353,7 +367,8 @@ PoseArray convertToGeometryPoseArray(const PathWithLaneId & path) PredictedPath convertToPredictedPath( const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const double duration, const double resolution, const double acceleration) + const double nearest_seg_idx, const double duration, const double resolution, + const double acceleration, const double min_speed) { PredictedPath predicted_path{}; predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); @@ -362,13 +377,11 @@ PredictedPath convertToPredictedPath( return predicted_path; } - const auto & geometry_points = convertToGeometryPointArray(path); FrenetCoordinate3d vehicle_pose_frenet = - convertToFrenetCoordinate3d(geometry_points, vehicle_pose.position); + convertToFrenetCoordinate3d(path.points, vehicle_pose.position, nearest_seg_idx); auto clock{rclcpp::Clock{RCL_ROS_TIME}}; rclcpp::Time start_time = clock.now(); double vehicle_speed = std::abs(vehicle_twist.linear.x); - constexpr double min_speed = 1.0; if (vehicle_speed < min_speed) { vehicle_speed = min_speed; RCLCPP_DEBUG_STREAM_THROTTLE( @@ -381,7 +394,7 @@ PredictedPath convertToPredictedPath( double prev_vehicle_speed = vehicle_speed; // first point - const auto pt = lerpByLength(geometry_points, vehicle_pose_frenet.length); + const auto pt = lerpByLength(path.points, vehicle_pose_frenet.length); Pose predicted_pose; predicted_pose.position = pt; predicted_path.path.push_back(predicted_pose); @@ -397,7 +410,7 @@ PredictedPath convertToPredictedPath( } length += travel_distance; - const auto pt = lerpByLength(geometry_points, vehicle_pose_frenet.length + length); + const auto pt = lerpByLength(path.points, vehicle_pose_frenet.length + length); Pose predicted_pose; predicted_pose.position = pt; predicted_path.path.push_back(predicted_pose); @@ -437,41 +450,6 @@ Pose lerpByPose(const Pose & p1, const Pose & p2, const double t) return pose; } -Point lerpByPoint(const Point & p1, const Point & p2, const double t) -{ - tf2::Vector3 v1, v2; - v1.setValue(p1.x, p1.y, p1.z); - v2.setValue(p2.x, p2.y, p2.z); - - const auto lerped_point = v1.lerp(v2, t); - - Point point; - point.x = lerped_point.x(); - point.y = lerped_point.y(); - point.z = lerped_point.z(); - return point; -} - -Point lerpByLength(const std::vector & point_array, const double length) -{ - Point lerped_point; - if (point_array.empty()) { - return lerped_point; - } - Point prev_pt = point_array.front(); - double accumulated_length = 0; - for (const auto & pt : point_array) { - const double distance = tier4_autoware_utils::calcDistance3d(prev_pt, pt); - if (accumulated_length + distance > length) { - return lerpByPoint(prev_pt, pt, (length - accumulated_length) / distance); - } - accumulated_length += distance; - prev_pt = pt; - } - - return point_array.back(); -} - bool lerpByTimeStamp(const PredictedPath & path, const double t_query, Pose * lerped_pt) { const rclcpp::Duration time_step(path.time_step); @@ -521,34 +499,6 @@ bool lerpByTimeStamp(const PredictedPath & path, const double t_query, Pose * le return false; } -bool lerpByDistance( - const behavior_path_planner::PullOutPath & path, const double & s, Pose * lerped_pt, - const lanelet::ConstLanelets & road_lanes) -{ - if (lerped_pt == nullptr) { - // ROS_WARN_STREAM_THROTTLE(1, "failed to lerp by distance due to nullptr pt"); - return false; - } - - for (size_t i = 1; i < path.path.points.size(); i++) { - const auto & pt = path.path.points.at(i).point.pose; - const auto & prev_pt = path.path.points.at(i - 1).point.pose; - if ( - (s >= lanelet::utils::getArcCoordinates(road_lanes, prev_pt).length) && - (s < lanelet::utils::getArcCoordinates(road_lanes, pt).length)) { - const double distance = lanelet::utils::getArcCoordinates(road_lanes, pt).length - - lanelet::utils::getArcCoordinates(road_lanes, prev_pt).length; - const auto offset = s - lanelet::utils::getArcCoordinates(road_lanes, prev_pt).length; - const auto ratio = offset / distance; - *lerped_pt = lerpByPose(prev_pt, pt, ratio); - return true; - } - } - - // ROS_ERROR_STREAM("Something failed in function: " << __func__); - return false; -} - double getDistanceBetweenPredictedPaths( const PredictedPath & object_path, const PredictedPath & ego_path, const double start_time, const double end_time, const double resolution) @@ -600,43 +550,40 @@ double getDistanceBetweenPredictedPathAndObject( return min_distance; } -double getDistanceBetweenPredictedPathAndObjectPolygon( - const PredictedObject & object, const PullOutPath & ego_path, - const tier4_autoware_utils::LinearRing2d & vehicle_footprint, double distance_resolution, - const lanelet::ConstLanelets & road_lanes) +bool checkCollisionBetweenPathFootprintsAndObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin) { - double min_distance = std::numeric_limits::max(); - - const auto ego_path_point_array = convertToGeometryPointArray(ego_path.path); - Polygon2d obj_polygon; - if (!calcObjectPolygon(object, &obj_polygon)) { - // ROS_ERROR("calcObjectPolygon failed"); - return min_distance; + for (const auto & p : ego_path.points) { + if (checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, p.point.pose, dynamic_objects, margin)) { + return true; + } } - const auto s_start = - lanelet::utils::getArcCoordinates(road_lanes, ego_path.path.points.front().point.pose).length; - const auto s_end = lanelet::utils::getArcCoordinates(road_lanes, ego_path.shift_point.end).length; + return false; +} - for (auto s = s_start + distance_resolution; s < s_end; s += distance_resolution) { - Pose ego_pose; - if (!lerpByDistance(ego_path, s, &ego_pose, road_lanes)) { - // ROS_ERROR("lerp failed"); +bool checkCollisionBetweenFootprintAndObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const PredictedObjects & dynamic_objects, const double margin) +{ + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); + + for (const auto & object : dynamic_objects.objects) { + Polygon2d obj_polygon; + if (!calcObjectPolygon(object, &obj_polygon)) { continue; } - const auto vehicle_footprint_on_path = - transformVector(vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); - Point2d ego_point{ego_pose.position.x, ego_pose.position.y}; - for (const auto & vehicle_footprint : vehicle_footprint_on_path) { - double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); - if (distance < min_distance) { - min_distance = distance; - } - } + + const double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); + if (distance < margin) return true; } - return min_distance; + return false; } + // only works with consecutive lanes -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, const double start_arc_length, const double end_arc_length) { @@ -677,7 +624,7 @@ std::vector filterObjectsByLanelets( } // works with random lanelets -std::vector filterObjectsByLanelets( +std::vector filterObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { std::vector indices; @@ -721,75 +668,36 @@ std::vector filterObjectsByLanelets( return indices; } +PredictedObjects filterObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) +{ + PredictedObjects filtered_objects; + const auto indices = filterObjectIndicesByLanelets(objects, target_lanelets); + for (const size_t i : indices) { + filtered_objects.objects.push_back(objects.objects.at(i)); + } + return filtered_objects; +} + bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon) { - const double obj_x = object.kinematics.initial_pose_with_covariance.pose.position.x; - const double obj_y = object.kinematics.initial_pose_with_covariance.pose.position.y; if (object.shape.type == Shape::BOUNDING_BOX) { - const double len_x = object.shape.dimensions.x; - const double len_y = object.shape.dimensions.y; - - tf2::Transform tf_map2obj; - tf2::fromMsg(object.kinematics.initial_pose_with_covariance.pose, tf_map2obj); - - // set vertices at map coordinate - tf2::Vector3 p1_map, p2_map, p3_map, p4_map; - - p1_map.setX(len_x / 2); - p1_map.setY(len_y / 2); - p1_map.setZ(0.0); - p1_map.setW(1.0); - - p2_map.setX(-len_x / 2); - p2_map.setY(len_y / 2); - p2_map.setZ(0.0); - p2_map.setW(1.0); - - p3_map.setX(-len_x / 2); - p3_map.setY(-len_y / 2); - p3_map.setZ(0.0); - p3_map.setW(1.0); - - p4_map.setX(len_x / 2); - p4_map.setY(-len_y / 2); - p4_map.setZ(0.0); - p4_map.setW(1.0); - - // transform vertices from map coordinate to object coordinate - tf2::Vector3 p1_obj, p2_obj, p3_obj, p4_obj; - - p1_obj = tf_map2obj * p1_map; - p2_obj = tf_map2obj * p2_map; - p3_obj = tf_map2obj * p3_map; - p4_obj = tf_map2obj * p4_map; - - object_polygon->outer().emplace_back(p1_obj.x(), p1_obj.y()); - object_polygon->outer().emplace_back(p2_obj.x(), p2_obj.y()); - object_polygon->outer().emplace_back(p3_obj.x(), p3_obj.y()); - object_polygon->outer().emplace_back(p4_obj.x(), p4_obj.y()); + const double & len_x = object.shape.dimensions.x; + const double & len_y = object.shape.dimensions.y; + *object_polygon = convertBoundingBoxObjectToGeometryPolygon( + object.kinematics.initial_pose_with_covariance.pose, len_x, len_y); } else if (object.shape.type == Shape::CYLINDER) { - const size_t N = 20; - const double r = object.shape.dimensions.x / 2; - for (size_t i = 0; i < N; ++i) { - object_polygon->outer().emplace_back( - obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); - } + *object_polygon = convertCylindricalObjectToGeometryPolygon( + object.kinematics.initial_pose_with_covariance.pose, object.shape); } else if (object.shape.type == Shape::POLYGON) { - tf2::Transform tf_map2obj; - tf2::fromMsg(object.kinematics.initial_pose_with_covariance.pose, tf_map2obj); - const auto obj_points = object.shape.footprint.points; - for (const auto & obj_point : obj_points) { - tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); - tf2::Vector3 tf_obj = tf_map2obj * obj; - object_polygon->outer().emplace_back(tf_obj.x(), tf_obj.y()); - } + *object_polygon = convertPolygonObjectToGeometryPolygon( + object.kinematics.initial_pose_with_covariance.pose, object.shape); } else { RCLCPP_WARN( rclcpp::get_logger("behavior_path_planner").get_child("utilities"), "Object shape unknown!"); return false; } - object_polygon->outer().push_back(object_polygon->outer().front()); return true; } @@ -816,7 +724,7 @@ std::vector calcObjectsDistanceToPath( return distance_array; } -std::vector filterObjectsByPath( +std::vector filterObjectsIndicesByPath( const PredictedObjects & objects, const std::vector & object_indices, const PathWithLaneId & ego_path, const double vehicle_width) { @@ -871,7 +779,7 @@ bool exists(std::vector vec, T element) return std::find(vec.begin(), vec.end(), element) != vec.end(); } -boost::optional findNearestIndexToGoal( +boost::optional findIndexOutOfGoalSearchRange( const std::vector & points, const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) @@ -880,17 +788,21 @@ boost::optional findNearestIndexToGoal( return boost::none; } + // find goal index size_t min_dist_index; double min_dist = std::numeric_limits::max(); { bool found = false; for (size_t i = 0; i < points.size(); ++i) { - const double x = points.at(i).point.pose.position.x - goal.position.x; - const double y = points.at(i).point.pose.position.y - goal.position.y; - const double dist = std::hypot(x, y); - if (dist < max_dist && dist < min_dist && exists(points.at(i).lane_ids, goal_lane_id)) { + const auto & lane_ids = points.at(i).lane_ids; + + const double dist_to_goal = + tier4_autoware_utils::calcDistance2d(points.at(i).point.pose, goal); + const bool is_goal_lane_id_in_point = + std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); + if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { min_dist_index = i; - min_dist = dist; + min_dist = dist_to_goal; found = true; } } @@ -899,16 +811,16 @@ boost::optional findNearestIndexToGoal( } } + // find index out of goal search range size_t min_dist_out_of_range_index = min_dist_index; - for (size_t i = min_dist_index; i != 0; --i) { - const double x = points.at(i).point.pose.position.x - goal.position.x; - const double y = points.at(i).point.pose.position.y - goal.position.y; - const double dist = std::hypot(x, y); + for (int i = min_dist_index; 0 <= i; --i) { + const double dist = tier4_autoware_utils::calcDistance2d(points.at(i).point, goal); min_dist_out_of_range_index = i; if (max_dist < dist) { break; } } + return min_dist_out_of_range_index; } @@ -927,7 +839,7 @@ bool setGoal( // NOTE: goal does not have valid z, that will be calculated by interpolation here PathPointWithLaneId refined_goal{}; const size_t closest_seg_idx_for_goal = - motion_utils::findNearestSegmentIndex(input.points, goal.position); + drivable_area_utils::findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); refined_goal.point.pose = goal; refined_goal.point.pose.position.z = calcInterpolatedZ(input, goal.position, closest_seg_idx_for_goal); @@ -939,34 +851,20 @@ bool setGoal( constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); - const size_t closest_seg_idx_for_pre_goal = - motion_utils::findNearestSegmentIndex(input.points, pre_refined_goal.point.pose.position); + const size_t closest_seg_idx_for_pre_goal = drivable_area_utils::findNearestSegmentIndex( + input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); pre_refined_goal.point.pose.position.z = calcInterpolatedZ(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); pre_refined_goal.point.longitudinal_velocity_mps = calcInterpolatedVelocity(input, closest_seg_idx_for_pre_goal); - // find min_dist_index whose distance to goal is shorter than search_radius_range - const auto min_dist_index_opt = - findNearestIndexToGoal(input.points, goal, goal_lane_id, search_radius_range); - if (!min_dist_index_opt) { - return false; - } - const size_t min_dist_index = - min_dist_index_opt.get(); // min_dist_idx point is inside the search_radius_range circle  - // find min_dist_out_of_circle_index whose distance to goal is longer than search_radius_range - size_t min_dist_out_of_circle_index = 0; - { - // NOTE: type of i must be int since i will be -1 even if the condition is 0<=i - for (int i = min_dist_index; 0 <= i; --i) { - const double dist = tier4_autoware_utils::calcDistance2d(input.points.at(i).point, goal); - min_dist_out_of_circle_index = i; - if (search_radius_range < dist) { - break; - } - } + const auto min_dist_out_of_circle_index_opt = + findIndexOutOfGoalSearchRange(input.points, goal, goal_lane_id, search_radius_range); + if (!min_dist_out_of_circle_index_opt) { + return false; } + const size_t min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.get(); // create output points for (size_t i = 0; i <= min_dist_out_of_circle_index; ++i) { @@ -1090,7 +988,8 @@ OccupancyGrid generateDrivableArea( // calculate min/max x and y from lanes in path argument (not from lanes argument) const auto path_scope = drivable_area_utils::getPathScope( path, route_handler, current_pose->pose, params.drivable_lane_forward_length, - params.drivable_lane_backward_length, params.drivable_lane_margin); + params.drivable_lane_backward_length, params.drivable_lane_margin, + params.ego_nearest_dist_threshold, params.ego_nearest_yaw_threshold); const double min_x = drivable_area_utils::quantize(path_scope.at(0) - params.drivable_area_margin, resolution); @@ -1797,6 +1696,7 @@ bool checkLaneIsInIntersection( return true; } +// for lane following PathWithLaneId setDecelerationVelocity( const RouteHandler & route_handler, const PathWithLaneId & input, const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration, @@ -1857,6 +1757,8 @@ PathWithLaneId setDecelerationVelocity( return reference_path; } +// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex inside the +// function PathWithLaneId setDecelerationVelocity( const PathWithLaneId & input, const double target_velocity, const Pose target_pose, const double buffer, const double deceleration_interval) @@ -1886,6 +1788,8 @@ PathWithLaneId setDecelerationVelocity( return reference_path; } +// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex inside the +// function PathWithLaneId setDecelerationVelocityForTurnSignal( const PathWithLaneId & input, const Pose target_pose, const double turn_light_on_threshold_time) { @@ -1996,4 +1900,416 @@ lanelet::ConstLanelets calcLaneAroundPose( return current_lanes; } + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & length, const double & width) +{ + Polygon2d object_polygon; + + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + + // set vertices at map coordinate + tf2::Vector3 p1_map; + p1_map.setX(length / 2); + p1_map.setY(width / 2); + p1_map.setZ(0.0); + p1_map.setW(1.0); + + tf2::Vector3 p2_map; + p2_map.setX(-length / 2); + p2_map.setY(width / 2); + p2_map.setZ(0.0); + p2_map.setW(1.0); + + tf2::Vector3 p3_map; + p3_map.setX(-length / 2); + p3_map.setY(-width / 2); + p3_map.setZ(0.0); + p3_map.setW(1.0); + + tf2::Vector3 p4_map; + p4_map.setX(length / 2); + p4_map.setY(-width / 2); + p4_map.setZ(0.0); + p4_map.setW(1.0); + + // transform vertices from map coordinate to object coordinate + tf2::Vector3 p1_obj = tf_map2obj * p1_map; + tf2::Vector3 p2_obj = tf_map2obj * p2_map; + tf2::Vector3 p3_obj = tf_map2obj * p3_map; + tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + + return object_polygon; +} + +Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + + return object_polygon; +} + +std::string getUuidStr(const PredictedObject & obj) +{ + std::stringstream hex_value; + for (const auto & uuid : obj.object_id.uuid) { + hex_value << std::hex << std::setfill('0') << std::setw(2) << +uuid; + } + return hex_value.str(); +} + +template +ProjectedDistancePoint pointToSegment( + const Point2d & reference_point, const Point2d & point_from_ego, + const Point2d & point_from_object) +{ + auto copied_point_from_object = point_from_object; + auto copied_point_from_reference = reference_point; + bg::subtract_point(copied_point_from_object, point_from_ego); + bg::subtract_point(copied_point_from_reference, point_from_ego); + + const auto c1 = bg::dot_product(copied_point_from_reference, copied_point_from_object); + if (!(c1 > 0)) { + return {point_from_ego, Pythagoras::apply(reference_point, point_from_ego)}; + } + + const auto c2 = bg::dot_product(copied_point_from_object, copied_point_from_object); + if (!(c2 > c1)) { + return {point_from_object, Pythagoras::apply(reference_point, point_from_object)}; + } + + Point2d projected = point_from_ego; + bg::multiply_value(copied_point_from_object, c1 / c2); + bg::add_point(projected, copied_point_from_object); + + return {projected, Pythagoras::apply(reference_point, projected)}; +} + +void getProjectedDistancePointFromPolygons( + const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, + Pose & point_on_object) +{ + ProjectedDistancePoint nearest; + std::unique_ptr current_point; + + bool points_in_ego{false}; + + const auto findPoints = [&nearest, ¤t_point, &points_in_ego]( + const Polygon2d & polygon_for_segment, + const Polygon2d & polygon_for_points, const bool & ego_is_points) { + const auto segments = boost::make_iterator_range( + bg::segments_begin(polygon_for_segment), bg::segments_end(polygon_for_segment)); + const auto points = boost::make_iterator_range( + bg::points_begin(polygon_for_points), bg::points_end(polygon_for_points)); + + for (auto && segment : segments) { + for (auto && point : points) { + const auto projected = pointToSegment(point, *segment.first, *segment.second); + if (!current_point || projected.distance < nearest.distance) { + current_point = std::make_unique(point); + nearest = projected; + points_in_ego = ego_is_points; + } + } + } + }; + + std::invoke(findPoints, ego_polygon, object_polygon, false); + std::invoke(findPoints, object_polygon, ego_polygon, true); + + if (!points_in_ego) { + point_on_object.position.x = current_point->x(); + point_on_object.position.y = current_point->y(); + point_on_ego.position.x = nearest.projected_point.x(); + point_on_ego.position.y = nearest.projected_point.y(); + } else { + point_on_ego.position.x = current_point->x(); + point_on_ego.position.y = current_point->y(); + point_on_object.position.x = nearest.projected_point.x(); + point_on_object.position.y = nearest.projected_point.y(); + } +} + +bool getEgoExpectedPoseAndConvertToPolygon( + const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose, + tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time, + const double & length, const double & width) +{ + if (!util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose)) { + return false; + } + expected_pose.orientation = current_pose.orientation; + ego_polygon = util::convertBoundingBoxObjectToGeometryPolygon(expected_pose, length, width); + + return true; +} + +bool getObjectExpectedPoseAndConvertToPolygon( + const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose, + Polygon2d & obj_polygon, const double & check_current_time) +{ + if (!util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose)) { + return false; + } + expected_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; + + return util::calcObjectPolygon(object, &obj_polygon); +} + +std::vector getPredictedPathFromObj( + const PredictedObject & obj, const bool & is_use_all_predicted_path) +{ + std::vector predicted_path_vec; + if (is_use_all_predicted_path) { + std::copy_if( + obj.kinematics.predicted_paths.cbegin(), obj.kinematics.predicted_paths.cend(), + std::back_inserter(predicted_path_vec), + [](const PredictedPath & path) { return !path.path.empty(); }); + } else { + const auto max_confidence_path = std::max_element( + obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), + [](const auto & path1, const auto & path2) { return path1.confidence > path2.confidence; }); + if (max_confidence_path != obj.kinematics.predicted_paths.end()) { + predicted_path_vec.push_back(*max_confidence_path); + } + } + return predicted_path_vec; +} + +Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object) +{ + tf2::Transform tf_map_desired_to_global{}; + tf2::Transform tf_map_target_to_global{}; + + tf2::fromMsg(desired_object, tf_map_desired_to_global); + tf2::fromMsg(target_object, tf_map_target_to_global); + + Pose desired_obj_pose_projected_to_target{}; + tf2::toMsg( + tf_map_desired_to_global.inverse() * tf_map_target_to_global, + desired_obj_pose_projected_to_target); + + return desired_obj_pose_projected_to_target; +} + +bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) +{ + const auto obj_from_ego = projectCurrentPoseToTarget(ego_pose, obj_pose); + return obj_from_ego.position.x > 0; +} + +bool isObjectFront(const Pose & projected_ego_pose) { return projected_ego_pose.position.x > 0; } + +double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel) +{ + const auto deceleration = (vehicle_accel < -1e-3) ? vehicle_accel : -1.0; + return -std::pow(vehicle_velocity, 2) / (2.0 * deceleration); +} + +double frontVehicleStopDistance( + const double & front_vehicle_velocity, const double & front_vehicle_accel, + const double & distance_to_collision) +{ + return stoppingDistance(front_vehicle_velocity, front_vehicle_accel) + distance_to_collision; +} + +double rearVehicleStopDistance( + const double & rear_vehicle_velocity, const double & rear_vehicle_accel, + const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin) +{ + return rear_vehicle_velocity * rear_vehicle_reaction_time + + stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel) + + rear_vehicle_velocity * rear_vehicle_safety_time_margin; +} + +bool isLongitudinalDistanceEnough( + const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold) +{ + return rear_vehicle_stop_threshold < front_vehicle_stop_threshold; +} + +bool hasEnoughDistance( + const Pose & expected_ego_pose, const Twist & ego_current_twist, + const Pose & expected_object_pose, const Twist & object_current_twist, + const BehaviorPathPlannerParameters & param, CollisionCheckDebug & debug) +{ + const auto front_vehicle_pose = + projectCurrentPoseToTarget(expected_ego_pose, expected_object_pose); + debug.relative_to_ego = front_vehicle_pose; + + if (isLateralDistanceEnough( + front_vehicle_pose.position.y, param.lateral_distance_max_threshold)) { + return true; + } + + const auto is_obj_in_front = isObjectFront(front_vehicle_pose); + debug.is_front = is_obj_in_front; + + const auto front_vehicle_velocity = + (is_obj_in_front) ? object_current_twist.linear : ego_current_twist.linear; + + const auto rear_vehicle_velocity = + (is_obj_in_front) ? ego_current_twist.linear : object_current_twist.linear; + + const auto front_vehicle_accel = param.expected_front_deceleration; + const auto rear_vehicle_accel = param.expected_rear_deceleration; + + const auto front_vehicle_stop_threshold = frontVehicleStopDistance( + util::l2Norm(front_vehicle_velocity), front_vehicle_accel, + std::fabs(front_vehicle_pose.position.x)); + + const auto rear_vehicle_stop_threshold = std::max( + rearVehicleStopDistance( + util::l2Norm(rear_vehicle_velocity), rear_vehicle_accel, param.rear_vehicle_reaction_time, + param.rear_vehicle_safety_time_margin), + param.longitudinal_distance_min_threshold); + + return isLongitudinalDistanceEnough(rear_vehicle_stop_threshold, front_vehicle_stop_threshold); +} + +bool isLateralDistanceEnough( + const double & relative_lateral_distance, const double & lateral_distance_threshold) +{ + return std::fabs(relative_lateral_distance) > lateral_distance_threshold; +} + +bool isSafeInLaneletCollisionCheck( + const Pose & ego_current_pose, const Twist & ego_current_twist, + const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, + const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const double & check_time_resolution, const PredictedObject & target_object, + const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, + CollisionCheckDebug & debug) +{ + const auto lerp_path_reserve = (check_end_time - check_start_time) / check_time_resolution; + if (lerp_path_reserve > 1e-3) { + debug.lerped_path.reserve(static_cast(lerp_path_reserve)); + } + + for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { + Pose expected_obj_pose; + tier4_autoware_utils::Polygon2d obj_polygon; + if (!util::getObjectExpectedPoseAndConvertToPolygon( + target_object_path, target_object, expected_obj_pose, obj_polygon, t)) { + debug.failed_reason = "get_obj_info_failed"; + continue; + } + + Pose expected_ego_pose; + tier4_autoware_utils::Polygon2d ego_polygon; + if (!util::getEgoExpectedPoseAndConvertToPolygon( + ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, + ego_vehicle_length, ego_vehicle_width)) { + debug.failed_reason = "get_ego_info_failed"; + continue; + } + + debug.ego_polygon = ego_polygon; + debug.obj_polygon = obj_polygon; + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + debug.failed_reason = "overlap_polygon"; + return false; + } + + debug.lerped_path.push_back(expected_ego_pose); + + util::getProjectedDistancePointFromPolygons( + ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); + debug.expected_ego_pose = expected_ego_pose; + debug.expected_obj_pose = expected_obj_pose; + + const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + if (!util::hasEnoughDistance( + expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, common_parameters, + debug)) { + debug.failed_reason = "not_enough_longitudinal"; + return false; + } + } + return true; +} + +bool isSafeInFreeSpaceCollisionCheck( + const Pose & ego_current_pose, const Twist & ego_current_twist, + const PredictedPath & ego_predicted_path, const double & ego_vehicle_length, + const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time, + const double & check_time_resolution, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug) +{ + tier4_autoware_utils::Polygon2d obj_polygon; + if (!util::calcObjectPolygon(target_object, &obj_polygon)) { + return false; + } + for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { + Pose expected_ego_pose; + tier4_autoware_utils::Polygon2d ego_polygon; + if (!util::getEgoExpectedPoseAndConvertToPolygon( + ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, + ego_vehicle_length, ego_vehicle_width)) { + debug.failed_reason = "get_ego_info_failed"; + continue; + } + + debug.ego_polygon = ego_polygon; + debug.obj_polygon = obj_polygon; + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + debug.failed_reason = "overlap_polygon"; + return false; + } + + auto expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; + util::getProjectedDistancePointFromPolygons( + ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); + + debug.expected_ego_pose = expected_ego_pose; + debug.expected_obj_pose = expected_obj_pose; + + const auto object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + if (!util::hasEnoughDistance( + expected_ego_pose, ego_current_twist, + target_object.kinematics.initial_pose_with_covariance.pose, object_twist, + common_parameters, debug)) { + debug.failed_reason = "not_enough_longitudinal"; + return false; + } + } + return true; +} } // namespace behavior_path_planner::util diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp new file mode 100644 index 0000000000000..fb5557170b39a --- /dev/null +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -0,0 +1,37 @@ +// 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 "behavior_path_planner/scene_module/lane_change/util.hpp" + +#include +#include + +TEST(BehaviorPathPlanningLaneChangeUtilsTest, testStoppingDistance) +{ + const auto vehicle_velocity = 8.333; + + const auto negative_accel = -1.5; + const auto distance_when_negative = + behavior_path_planner::lane_change_utils::stoppingDistance(vehicle_velocity, negative_accel); + ASSERT_NEAR(distance_when_negative, 23.1463, 1e-3); + + const auto positive_accel = 1.5; + const auto distance_when_positive = + behavior_path_planner::lane_change_utils::stoppingDistance(vehicle_velocity, positive_accel); + ASSERT_NEAR(distance_when_positive, 34.7194, 1e-3); + + const auto zero_accel = 0.0; + const auto distance_when_zero = + behavior_path_planner::lane_change_utils::stoppingDistance(vehicle_velocity, zero_accel); + ASSERT_NEAR(distance_when_zero, 34.7194, 1e-3); +} diff --git a/planning/behavior_path_planner/test/test_utilities.cpp b/planning/behavior_path_planner/test/test_utilities.cpp index 3b9ea63e490d6..71882d96e8b38 100644 --- a/planning/behavior_path_planner/test/test_utilities.cpp +++ b/planning/behavior_path_planner/test/test_utilities.cpp @@ -25,12 +25,14 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnStraightLin { PathWithLaneId path = behavior_path_planner::generateStraightSamplePathWithLaneId(0.0f, 1.0f, 10u); - std::vector geometry_points = - behavior_path_planner::util::convertToGeometryPointArray(path); + std::vector geometry_points = + behavior_path_planner::util::convertToPoseArray(path); Pose vehicle_pose = behavior_path_planner::generateEgoSamplePose(10.7f, -1.7f, 0.0); + const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + geometry_points, vehicle_pose, 3.0, 1.0); FrenetCoordinate3d vehicle_pose_frenet = behavior_path_planner::util::convertToFrenetCoordinate3d( - geometry_points, vehicle_pose.position); + geometry_points, vehicle_pose.position, vehicle_seg_idx); EXPECT_NEAR(vehicle_pose_frenet.distance, -1.7f, 1e-3); EXPECT_NEAR(vehicle_pose_frenet.length, 10.7f, 1e-3); @@ -40,12 +42,14 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnDiagonalLin { PathWithLaneId path = behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 10u); - std::vector geometry_points = - behavior_path_planner::util::convertToGeometryPointArray(path); + std::vector geometry_points = + behavior_path_planner::util::convertToPoseArray(path); Pose vehicle_pose = behavior_path_planner::generateEgoSamplePose(0.1f, 0.1f, 0.0); + const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + geometry_points, vehicle_pose, 3.0, 1.0); FrenetCoordinate3d vehicle_pose_frenet = behavior_path_planner::util::convertToFrenetCoordinate3d( - geometry_points, vehicle_pose.position); + geometry_points, vehicle_pose.position, vehicle_seg_idx); EXPECT_NEAR(vehicle_pose_frenet.distance, 0, 1e-2); EXPECT_NEAR(vehicle_pose_frenet.length, 0.1414f, 1e-2); diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index eb07deaa2ce23..0ea817ffefc81 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -3,6 +3,8 @@ intersection: state_transit_margin_time: 0.5 stop_line_margin: 3.0 + keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr + keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml index 936bfc76cc98d..be7b0e7e70bb4 100644 --- a/planning/behavior_velocity_planner/config/stop_line.param.yaml +++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml @@ -2,8 +2,8 @@ ros__parameters: stop_line: stop_margin: 0.0 - stop_check_dist: 2.0 stop_duration_sec: 1.0 + hold_stop_margin_distance: 2.0 use_initialization_stop_line_state: true debug: show_stopline_collision_check: false # [-] whether to show stopline collision diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp index 1e19a0d4f256b..8c95b6d128992 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp @@ -78,6 +78,10 @@ struct PlannerData // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + // nearest search + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + // other internal data std::map traffic_light_id_map; // external data diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index d3b27163d4c88..4ee5d9d40dc4e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -123,7 +123,7 @@ class BlindSpotModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; private: - int64_t lane_id_; + const int64_t lane_id_; TurnDirection turn_direction_; bool has_traffic_light_; bool is_over_pass_judge_line_; diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 4d48f8d787853..08da219f9bea7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -66,7 +66,8 @@ class DetectionAreaModule : public SceneModuleInterface public: DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); @@ -85,6 +86,9 @@ class DetectionAreaModule : public SceneModuleInterface bool hasEnoughBrakingDistance( const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + // Lane id + int64_t lane_id_; + // Key Feature const lanelet::autoware::DetectionArea & detection_area_reg_elem_; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 1879b7178e120..3cb169c981e0f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -105,6 +105,10 @@ class IntersectionModule : public SceneModuleInterface { double state_transit_margin_time; double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double keep_detection_line_margin; //! distance (toward path end) from generated stop line. + //! keep detection if ego is before this line and ego.vel < + //! keep_detection_vel_thr + double keep_detection_vel_thr; double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check @@ -235,24 +239,6 @@ class IntersectionModule : public SceneModuleInterface bool isTargetStuckVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - /** - * @brief convert object to footprint polygon - * @param object detected object - * @return 2d polygon of the object footprint - */ - Polygon2d toFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; - - /** - * @brief convert predicted object to footprint polygon - * @param object detected object - * @param predicted_pose predicted object pose - * @return 2d polygon of the object footprint - */ - Polygon2d toPredictedFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const geometry_msgs::msg::Pose & predicted_pose) const; - /** * @brief Whether target tier4_api_msgs::Intersection::status is valid or not * @param target_status target tier4_api_msgs::Intersection::status diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index e228deec9d704..94a5d2fac2095 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -39,7 +39,7 @@ int insertPoint( autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id); -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx); @@ -54,6 +54,14 @@ bool getObjectiveLanelets( std::vector * objective_lanelets_with_margin_result, const rclcpp::Logger logger); +struct StopLineIdx +{ + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; +}; + /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, * read it from the map; otherwise, generate a stop line at a position where it will not collide. @@ -68,9 +76,10 @@ bool getObjectiveLanelets( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger); + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger); /** * @brief If use_stuck_stopline is true, a stop line is generated before the intersection. @@ -118,6 +127,22 @@ std::vector getLaneletIdsFromLaneletsVec(const std::vector & possible_collisions, double offset); diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp index a6e99c6d503c3..bb6fc7c237418 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp @@ -90,15 +90,15 @@ class RunOutModule : public SceneModuleInterface const float velocity_mps) const; bool checkCollisionWithShape( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const Shape & shape, std::vector & collision_points) const; + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + std::vector & collision_points) const; bool checkCollisionWithCylinder( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const float radius, std::vector & collision_points) const; + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius, + std::vector & collision_points) const; bool checkCollisionWithBoundingBox( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const geometry_msgs::msg::Vector3 & dimension, std::vector & collision_points) const; diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index 06586ac3b8c7f..e36dd38516890 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -37,6 +37,7 @@ #include #include #include +#include // Debug #include @@ -110,6 +111,22 @@ class SceneModuleInterface void setSafe(const bool safe) { safe_ = safe; } void setDistance(const double distance) { distance_ = distance; } + + template + size_t findEgoSegmentIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } + + template + size_t findEgoIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } }; class SceneModuleManagerInterface @@ -261,6 +278,23 @@ class SceneModuleManagerInterface scene_modules_.erase(scene_module); } + template + size_t findEgoSegmentIndex(const std::vector & points) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } + + template + size_t findEgoIndex( + const std::vector & points, const geometry_msgs::msg::Pose & ego_pose) const + { + const auto & p = planner_data_; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, p->current_pose.pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); + } + std::set> scene_modules_; std::set registered_module_id_set_; diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp index d109398e33d56..c3faeac33d5e1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp @@ -15,6 +15,7 @@ #ifndef SCENE_MODULE__STOP_LINE__SCENE_HPP_ #define SCENE_MODULE__STOP_LINE__SCENE_HPP_ +#include #include #include #include @@ -70,8 +71,8 @@ class StopLineModule : public SceneModuleInterface struct PlannerParam { double stop_margin; - double stop_check_dist; double stop_duration_sec; + double hold_stop_margin_distance; bool use_initialization_stop_line_state; bool show_stopline_collision_check; }; @@ -90,29 +91,15 @@ class StopLineModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; private: - int64_t module_id_; + std::shared_ptr stopped_time_; geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); - boost::optional findCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index); - - boost::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPoint2d & collision); - - boost::optional calcStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const boost::optional & offset_segment); - - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPose & insert_index_with_pose, - tier4_planning_msgs::msg::StopReason * stop_reason); + int64_t lane_id_; lanelet::ConstLineString3d stop_line_; - int64_t lane_id_; + + // State machine State state_; // Parameter diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp index ace832e44ab38..56c73f3e1699f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp @@ -66,8 +66,9 @@ class TrafficLightModule : public SceneModuleInterface public: TrafficLightModule( - const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity( @@ -123,6 +124,9 @@ class TrafficLightModule : public SceneModuleInterface autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState( const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const; + // Lane id + const int64_t lane_id_; + // Key Feature const lanelet::TrafficLight & traffic_light_reg_elem_; lanelet::ConstLanelet lane_; diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 7f8e81891c3e4..24f628927e21b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -75,8 +75,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface public: VirtualTrafficLightModule( - const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity( @@ -87,7 +88,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; private: - const int64_t module_id_; + const int64_t lane_id_; const lanelet::autoware::VirtualTrafficLight & reg_elem_; const lanelet::ConstLanelet lane_; const PlannerParam planner_param_; @@ -101,13 +102,15 @@ class VirtualTrafficLightModule : public SceneModuleInterface void setStopReason( const geometry_msgs::msg::Pose & stop_pose, tier4_planning_msgs::msg::StopReason * stop_reason); - bool isBeforeStartLine(); + boost::optional getPathIndexOfFirstEndLine(); - bool isBeforeStopLine(); + bool isBeforeStartLine(const size_t end_line_idx); - bool isAfterAnyEndLine(); + bool isBeforeStopLine(const size_t end_line_idx); - bool isNearAnyEndLine(); + bool isAfterAnyEndLine(const size_t end_line_idx); + + bool isNearAnyEndLine(const size_t end_line_idx); boost::optional findCorrespondingState(); @@ -117,11 +120,11 @@ class VirtualTrafficLightModule : public SceneModuleInterface void insertStopVelocityAtStopLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason); + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); void insertStopVelocityAtEndLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason); + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; } // namespace behavior_velocity_planner #endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp index 55e16bd77a3a5..dc282785f330d 100644 --- a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp @@ -43,11 +43,53 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; + +namespace +{ +geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p) +{ + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x(); + geom_p.y = p.y(); + + return geom_p; +} + +geometry_msgs::msg::Point operator+( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + geometry_msgs::msg::Point p; + p.x = p1.x + p2.x; + p.y = p1.y + p2.y; + p.z = p1.z + p2.z; + + return p; +} + +[[maybe_unused]] geometry_msgs::msg::Point operator*( + const geometry_msgs::msg::Point & p, const double v) +{ + geometry_msgs::msg::Point multiplied_p; + multiplied_p.x = p.x * v; + multiplied_p.y = p.y * v; + multiplied_p.z = p.z * v; + + return multiplied_p; +} + +[[maybe_unused]] geometry_msgs::msg::Point operator*( + const double v, const geometry_msgs::msg::Point & p) +{ + return p * v; +} +} // namespace + namespace arc_lane_utils { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d -using PathIndexWithOffset = std::pair; // front index, offset +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithPoint = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset inline double calcSignedDistance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) @@ -58,51 +100,71 @@ inline double calcSignedDistance( return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); } -inline boost::optional getNearestCollisionPoint( - const LineString2d & stop_line, const LineString2d & path_segment) +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) +inline boost::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) { - // Find all collision points - std::vector collision_points; - bg::intersection(stop_line, path_segment, collision_points); - if (collision_points.empty()) { - return {}; + const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); + + if (det == 0.0) { + // collision is not one point. + return boost::none; } - // To dist list - std::vector dist_list; - dist_list.reserve(collision_points.size()); - std::transform( - collision_points.cbegin(), collision_points.cend(), std::back_inserter(dist_list), - [&path_segment](const Point2d & collision_point) { - return bg::distance(path_segment.front(), collision_point); - }); + const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; + const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; - // Find nearest collision point - const auto min_itr = std::min_element(dist_list.cbegin(), dist_list.cend()); - const auto min_idx = std::distance(dist_list.cbegin(), min_itr); + // check collision is outside the segment line + if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { + return boost::none; + } - return collision_points.at(min_idx); + return p1 * (1.0 - t1) + p2 * t1; } template -boost::optional findCollisionSegment( - const T & path, const LineString2d & stop_line) +boost::optional findCollisionSegment( + const T & path, const geometry_msgs::msg::Point & stop_line_p1, + const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id) { for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point - const auto & p2 = path.points.at(i + 1).point.pose.position; // Point after collision point + const auto & prev_lane_ids = path.points.at(i).lane_ids; + const auto & next_lane_ids = path.points.at(i + 1).lane_ids; + + const bool is_target_lane_in_prev_lane = + std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) != prev_lane_ids.end(); + const bool is_target_lane_in_next_lane = + std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != next_lane_ids.end(); + if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) { + continue; + } - const LineString2d path_segment = {{p1.x, p1.y}, {p2.x, p2.y}}; + const auto & p1 = + tier4_autoware_utils::getPoint(path.points.at(i)); // Point before collision point + const auto & p2 = + tier4_autoware_utils::getPoint(path.points.at(i + 1)); // Point after collision point - const auto nearest_collision_point = getNearestCollisionPoint(stop_line, path_segment); - if (nearest_collision_point) { - return std::make_pair(i, *nearest_collision_point); + const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); + + if (collision_point) { + return std::make_pair(i, collision_point.get()); } } return {}; } +template +boost::optional findCollisionSegment( + const T & path, const LineString2d & stop_line, const size_t target_lane_id) +{ + const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); + const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1)); + + return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_id); +} + template boost::optional findForwardOffsetSegment( const T & path, const size_t base_idx, const double offset_length) @@ -143,20 +205,22 @@ boost::optional findBackwardOffsetSegment( template boost::optional findOffsetSegment( - const T & path, const PathIndexWithPoint2d & collision_segment, const double offset_length) + const T & path, const PathIndexWithPoint & collision_segment, const double offset_length) { const size_t collision_idx = collision_segment.first; - const Point2d & collision_point = collision_segment.second; - const auto p_front = to_bg2d(path.points.at(collision_idx).point.pose.position); - const auto p_back = to_bg2d(path.points.at(collision_idx + 1).point.pose.position); + const auto & collision_point = collision_segment.second; if (offset_length >= 0) { return findForwardOffsetSegment( - path, collision_idx, offset_length + bg::distance(p_front, collision_point)); - } else { - return findBackwardOffsetSegment( - path, collision_idx + 1, -offset_length + bg::distance(p_back, collision_point)); + path, collision_idx, + offset_length + + tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx), collision_point)); } + + return findBackwardOffsetSegment( + path, collision_idx + 1, + -offset_length + + tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } inline boost::optional findOffsetSegment( @@ -201,10 +265,10 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse inline boost::optional createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const double margin, const double vehicle_offset) + const size_t lane_id, const double margin, const double vehicle_offset) { // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line); + const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); if (!collision_segment) { // No collision return {}; diff --git a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp index ffbab904aaf85..51296737ea768 100644 --- a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp @@ -15,6 +15,8 @@ #ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#include + #include #include #include @@ -33,7 +35,6 @@ #include #include #include -#include #include #include @@ -60,16 +61,16 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( namespace behavior_velocity_planner { -using Point2d = boost::geometry::model::d2::point_xy; -using Segment2d = boost::geometry::model::segment; -using LineString2d = boost::geometry::model::linestring; -using Polygon2d = - boost::geometry::model::polygon; // counter-clockwise, open +namespace bg = boost::geometry; + +using Point2d = tier4_autoware_utils::Point2d; +using LineString2d = tier4_autoware_utils::LineString2d; +using Polygon2d = tier4_autoware_utils::Polygon2d; template Point2d to_bg2d(const T & p) { - return Point2d(boost::geometry::get<0>(p), boost::geometry::get<1>(p)); + return Point2d(bg::get<0>(p), bg::get<1>(p)); } template @@ -82,17 +83,6 @@ LineString2d to_bg2d(const std::vector & vec) return ps; } -inline Polygon2d linestring2polygon(const LineString2d & line_string) -{ - Polygon2d polygon; - - for (const auto & p : line_string) { - polygon.outer().push_back(p); - } - - return polygon; -} - inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) { Polygon2d polygon; @@ -107,78 +97,11 @@ inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2 polygon.outer().push_back(*itr); } + bg::correct(polygon); return polygon; } -inline Polygon2d obj2polygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & shape) -{ - // rename - const double x = pose.position.x; - const double y = pose.position.y; - const double h = shape.x; - const double w = shape.y; - const double yaw = tf2::getYaw(pose.orientation); - - // create base polygon - Polygon2d obj_poly; - boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( - -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); - - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} - -inline double calcOverlapAreaRate(const Polygon2d & target, const Polygon2d & base) -{ - /* OverlapAreaRate: common area(target && base) / target area */ - - if (boost::geometry::within(target, base)) { - // target is within base, common area = target area - return 1.0; - } - - if (!boost::geometry::intersects(target, base)) { - // target and base has not intersect area - return 0.0; - } - - // calculate intersect polygon - std::vector intersects; - boost::geometry::intersection(target, base, intersects); - - // calculate area of polygon - double intersect_area = 0.0; - for (const auto & intersect : intersects) { - intersect_area += boost::geometry::area(intersect); - } - const double target_area = boost::geometry::area(target); - // specification of boost1.65 - // common area is not intersect area - const double common_area = target_area - intersect_area; - - return common_area / target_area; -} - -inline std::vector makeSegments(const LineString2d & ls) -{ - std::vector segments; - for (size_t i = 0; i < ls.size(); ++i) { - segments.emplace_back(ls.at(i), ls.at(i + 1)); - } - return segments; -} - -inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon) +inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) { geometry_msgs::msg::Polygon polygon_msg; geometry_msgs::msg::Point32 point_msg; @@ -189,27 +112,6 @@ inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon) } return polygon_msg; } - -inline Polygon2d toBoostPoly(const geometry_msgs::msg::Polygon & polygon) -{ - Polygon2d boost_poly; - for (const auto & point : polygon.points) { - const Point2d point2d(point.x, point.y); - boost_poly.outer().push_back(point2d); - } - return boost_poly; -} - -inline Polygon2d toBoostPoly(const lanelet::BasicPolygon2d & polygon) -{ - Polygon2d boost_poly; - for (const auto & vec : polygon) { - const Point2d point2d(vec.x(), vec.y()); - boost_poly.outer().push_back(point2d); - } - - return boost_poly; -} } // namespace behavior_velocity_planner #endif // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index aa5eecc245de6..3f6474ffbf895 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -85,58 +85,16 @@ inline Quaternion lerpOrientation( return tf2::toMsg(q_interpolated); } -/** - * @brief apply linear interpolation to trajectory point that is nearest to a certain point - * @param [in] points trajectory points - * @param [in] point Interpolated point is nearest to this point. - */ -template -boost::optional getLerpTrajectoryPointWithIdx( - const T & points, const geometry_msgs::msg::Point & point) -{ - TrajectoryPoint interpolated_point; - const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(points, point); - const double len_to_interpolated = - motion_utils::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); - const double len_segment = - motion_utils::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); - const double ratio = len_to_interpolated / len_segment; - if (ratio <= 0.0 || 1.0 <= ratio) return boost::none; - const double interpolate_ratio = std::clamp(ratio, 0.0, 1.0); - { - const size_t i = nearest_seg_idx; - const auto & pos0 = points.at(i).pose.position; - const auto & pos1 = points.at(i + 1).pose.position; - interpolated_point.pose.position.x = interpolation::lerp(pos0.x, pos1.x, interpolate_ratio); - interpolated_point.pose.position.y = interpolation::lerp(pos0.y, pos1.y, interpolate_ratio); - interpolated_point.pose.position.z = interpolation::lerp(pos0.z, pos1.z, interpolate_ratio); - interpolated_point.pose.orientation = lerpOrientation( - points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); - interpolated_point.longitudinal_velocity_mps = interpolation::lerp( - points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, - interpolate_ratio); - interpolated_point.lateral_velocity_mps = interpolation::lerp( - points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio); - interpolated_point.acceleration_mps2 = interpolation::lerp( - points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio); - interpolated_point.heading_rate_rps = interpolation::lerp( - points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); - } - return std::make_pair(interpolated_point, nearest_seg_idx); -} - //! smooth path point with lane id starts from ego position on path to the path end inline bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, const std::shared_ptr & planner_data) { - using motion_utils::findNearestIndex; const geometry_msgs::msg::Pose current_pose = planner_data->current_pose.pose; const double v0 = planner_data->current_velocity->twist.linear.x; const double a0 = planner_data->current_accel.get(); const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; - const double max = std::numeric_limits::max(); auto trajectory = convertPathToTrajectoryPoints(in_path); if (external_v_limit) { @@ -147,24 +105,24 @@ inline bool smoothPath( // Resample trajectory with ego-velocity based interval distances auto traj_resampled = smoother->resampleTrajectory( - *traj_lateral_acc_filtered, v0, current_pose, std::numeric_limits::max()); - const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4); - if (!traj_resampled_closest) { - return false; - } + *traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); + const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( + *traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); std::vector debug_trajectories; // Clip trajectory from closest point TrajectoryPoints clipped; TrajectoryPoints traj_smoothed; clipped.insert( - clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end()); + clipped.end(), traj_resampled->begin() + traj_resampled_closest, traj_resampled->end()); if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; return false; } traj_smoothed.insert( traj_smoothed.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); + traj_resampled->begin() + traj_resampled_closest); out_path = convertTrajectoryPointsToPath(traj_smoothed); return true; diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 0a3f410aa0021..22c75ddeaab50 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -79,7 +79,6 @@ struct PointWithSearchRangeIndex using geometry_msgs::msg::Pose; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using Point2d = boost::geometry::model::d2::point_xy; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::Shape; @@ -90,8 +89,6 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; using motion_utils::validateNonEmpty; using tier4_autoware_utils::calcAzimuthAngle; using tier4_autoware_utils::calcDistance2d; @@ -104,10 +101,48 @@ using tier4_planning_msgs::msg::StopReason; namespace planning_utils { +template +size_t calcPointIndexFromSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) +{ + const size_t prev_point_idx = seg_idx; + const size_t next_point_idx = seg_idx + 1; + + const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx)); + const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx)); + + if (prev_dist < next_dist) { + return prev_point_idx; + } + return next_point_idx; +} + +template +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) +{ + if (idx == 0) { + return 0; + } + if (idx == points.size() - 1) { + return idx - 1; + } + if (points.size() < 3) { + return 0; + } + + const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + if (0 < offset_to_seg) { + return idx; + } + return idx - 1; +} + // create detection area from given range return false if creation failure bool createDetectionAreaPolygons( - Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & current_pose, - const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity = 1.0); + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity = 1.0); PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio); Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y); void extractClosePartition( @@ -125,90 +160,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( geometry_msgs::msg::Pose transformAbsCoordinate2D( const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin); SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id); -/** - * @brief find nearest segment index with search range - */ -template -size_t findNearestSegmentIndex(const T & points, const PointWithSearchRangeIndex & point_with_index) -{ - const auto & index = point_with_index.index; - const auto point = point_with_index.point; - - validateNonEmpty(points); - - double min_dist = std::numeric_limits::max(); - size_t nearest_idx = 0; - for (size_t i = index.min_idx; i <= index.max_idx; ++i) { - const auto dist = calcSquaredDistance2d(points.at(i), point); - if (dist < min_dist) { - min_dist = dist; - nearest_idx = i; - } - } - - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == points.size() - 1) { - return points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} -/** - * @brief find nearest segment index within distance threshold - */ -template -PointWithSearchRangeIndex findFirstNearSearchRangeIndex( - const T & points, const geometry_msgs::msg::Point & point, const double distance_thresh = 9.0) -{ - validateNonEmpty(points); - - bool min_idx_found = false; - bool max_idx_found = false; - PointWithSearchRangeIndex point_with_range = {point, {static_cast(0), points.size() - 1}}; - for (size_t i = 0; i < points.size(); i++) { - const auto & p = points.at(i).point.pose.position; - const double dist = std::hypot(point.x - p.x, point.y - p.y); - if (dist < distance_thresh) { - if (!min_idx_found) { - point_with_range.index.min_idx = i; - min_idx_found = true; - } - if (!max_idx_found) point_with_range.index.max_idx = i; - } else if (min_idx_found) { - // found close index and farther than distance_thresh, stop update max index - max_idx_found = true; - } - } - return point_with_range; -} -/** - * @brief calcSignedArcLength from point to point with search range - */ -template -double calcSignedArcLengthWithSearchIndex( - const T & points, const PointWithSearchRangeIndex & src_point_with_range, - const PointWithSearchRangeIndex & dst_point_with_range) -{ - validateNonEmpty(points); - const size_t src_idx = planning_utils::findNearestSegmentIndex(points, src_point_with_range); - const size_t dst_idx = planning_utils::findNearestSegmentIndex(points, dst_point_with_range); - const double signed_length = calcSignedArcLength(points, src_idx, dst_idx); - const double signed_length_src_offset = - calcLongitudinalOffsetToSegment(points, src_idx, src_point_with_range.point); - const double signed_length_dst_offset = - calcLongitudinalOffsetToSegment(points, dst_idx, dst_point_with_range.point); - return signed_length - signed_length_src_offset + signed_length_dst_offset; -} -Polygon2d toFootprintPolygon(const PredictedObject & object); bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, @@ -254,7 +206,7 @@ std::vector concatVector(const std::vector & vec1, const std::vector & boost::optional getNearestLaneId( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, - const geometry_msgs::msg::Pose & current_pose, boost::optional & nearest_segment_idx); + const geometry_msgs::msg::Pose & current_pose); std::vector getSortedLaneIdsFromPath(const PathWithLaneId & path); @@ -270,9 +222,7 @@ std::unordered_map, lanelet::ConstLanelet> get std::unordered_map, lanelet::ConstLanelet> reg_elem_map_on_path; // Add current lane id - boost::optional nearest_segment_idx; - const auto nearest_lane_id = - getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx); + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { @@ -338,6 +288,8 @@ bool isOverLine( boost::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); +boost::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); } // namespace planning_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 139e5bc79312d..70d85b2e5d340 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -4,9 +4,39 @@ behavior_velocity_planner 0.1.0 The behavior_velocity_planner package - Yukihiro Saito + + + Mamoru Sobue + + Satoshi Ota + + Kyoichi Sugahara + + Taiki Tanaka + + Kosuke Takeuchi + + Satoshi Ota + + Mamoru Sobue + + Yutaka Shimizu + + Kosuke Takeuchi + + Tomohito Ando + Apache License 2.0 + Taiki Tanaka + Mamoru Sobue + Satoshi Ota + Kyoichi Sugahara + Kosuke Takeuchi + Yutaka Shimizu + Tomohito Ando + Yukihiro Saito + ament_cmake eigen3_cmake_module diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 587716670e62d..2ac0e6eb67f71 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -163,6 +163,12 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_data_.accel_lowpass_gain_ = this->declare_parameter("lowpass_gain", 0.5); planner_data_.stop_line_extend_length = this->declare_parameter("stop_line_extend_length", 5.0); + // nearest search + planner_data_.ego_nearest_dist_threshold = + this->declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = + this->declare_parameter("ego_nearest_yaw_threshold"); + // Initialize PlannerManager if (this->declare_parameter("launch_crosswalk", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index 200cf873ad4b4..956655d38aecf 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -30,12 +30,9 @@ std::vector getCrosswalksOnPath( { std::vector crosswalks; - auto nearest_segment_idx = motion_utils::findNearestSegmentIndex( - path.points, current_pose, std::numeric_limits::max(), M_PI_2); - // Add current lane id - const auto nearest_lane_id = behavior_velocity_planner::planning_utils::getNearestLaneId( - path, lanelet_map, current_pose, nearest_segment_idx); + const auto nearest_lane_id = + behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index f9bee714d7aff..0d6c838a4fcae 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -49,10 +49,11 @@ void DetectionAreaModuleManager::launchNewModules( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose)) { // Use lanelet_id to unregister module when the route is changed + const auto lane_id = detection_area_with_lane_id.second.id(); const auto module_id = detection_area_with_lane_id.first->id(); if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *detection_area_with_lane_id.first, planner_param_, + module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, logger_.get_child("detection_area_module"), clock_)); generateUUID(module_id); } diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 1573f5f576b6f..d89bb16b8b3ce 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -34,14 +34,14 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; -using motion_utils::findNearestSegmentIndex; -using motion_utils::insertTargetPoint; DetectionAreaModule::DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), planner_param_(planner_param) @@ -77,19 +77,27 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Get self pose const auto & self_pose = planner_data_->current_pose.pose; + const size_t current_seg_idx = findEgoSegmentIndex(path->points); // Get stop point const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, planner_param_.stop_margin, + original_path, stop_line, lane_id_, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { return true; } - const auto is_stopped = planner_data_->isVehicleStopped(0.0); + const auto & stop_point_idx = stop_point->first; + const auto & stop_pose = stop_point->second; + const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, stop_pose.position, stop_point_idx); + + auto modified_stop_pose = stop_pose; + size_t modified_stop_line_seg_idx = stop_line_seg_idx; - auto stop_pose = stop_point->second; - const auto stop_dist = calcSignedArcLength(path->points, self_pose.position, stop_pose.position); + const auto is_stopped = planner_data_->isVehicleStopped(0.0); + const auto stop_dist = calcSignedArcLength( + path->points, self_pose.position, current_seg_idx, stop_pose.position, stop_line_seg_idx); // Don't re-approach when the ego stops closer to the stop point than hold_stop_margin_distance if (is_stopped && stop_dist < planner_param_.hold_stop_margin_distance) { @@ -100,7 +108,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } - stop_pose = ego_pos_on_path.get(); + modified_stop_pose = ego_pos_on_path.get(); + modified_stop_line_seg_idx = current_seg_idx; } setDistance(stop_dist); @@ -117,14 +126,22 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (planner_param_.use_dead_line) { // Use '-' for margin because it's the backward distance from stop line const auto dead_line_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, -planner_param_.dead_line_margin, + original_path, stop_line, lane_id_, -planner_param_.dead_line_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (dead_line_point) { + const size_t dead_line_point_idx = dead_line_point->first; const auto & dead_line_pose = dead_line_point->second; + + const size_t dead_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, dead_line_pose.position, dead_line_point_idx); + debug_data_.dead_line_poses.push_back(dead_line_pose); - if (planning_utils::isOverLine(original_path, self_pose, dead_line_pose)) { + const double dist_from_ego_to_dead_line = calcSignedArcLength( + original_path.points, self_pose.position, current_seg_idx, dead_line_pose.position, + dead_line_seg_idx); + if (dist_from_ego_to_dead_line < 0.0) { RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); setSafe(true); return true; @@ -133,9 +150,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } // Ignore objects detected after stop_line if not in STOP state - if ( - state_ != State::STOP && - planning_utils::isOverLine(original_path, self_pose, stop_point->second)) { + const double dist_from_ego_to_stop = calcSignedArcLength( + original_path.points, self_pose.position, current_seg_idx, stop_pose.position, + stop_line_seg_idx); + if (state_ != State::STOP && dist_from_ego_to_stop < 0.0) { setSafe(true); return true; } @@ -153,7 +171,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Insert stop point state_ = State::STOP; - planning_utils::insertStopPoint(stop_pose.position, *path); + planning_utils::insertStopPoint(modified_stop_pose.position, modified_stop_line_seg_idx, *path); // For virtual wall debug_data_.stop_poses.push_back(stop_point->second); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index f4f389ad5e45a..df4cfc2051056 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -34,6 +34,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + ip.keep_detection_line_margin = node.declare_parameter(ns + ".keep_detection_line_margin", 1.0); + ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833); ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index f8a66a06db665..1e326f4975cd0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -122,17 +122,19 @@ bool IntersectionModule::modifyPathVelocity( debug_data_.detection_area_with_margin = detection_areas_with_margin; /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; - int pass_judge_line_idx = -1; - int first_idx_inside_lane = -1; + util::StopLineIdx stop_line_idxs; if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, *path, - &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs, + logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); return false; } + const int stop_line_idx = stop_line_idxs.stop_line_idx; + const int pass_judge_line_idx = stop_line_idxs.pass_judge_line_idx; + const int keep_detection_line_idx = stop_line_idxs.keep_detection_line_idx; if (stop_line_idx <= 0) { RCLCPP_DEBUG(logger_, "stop line line is at path[0], ignore planning."); RCLCPP_DEBUG(logger_, "===== plan end ====="); @@ -152,21 +154,32 @@ bool IntersectionModule::modifyPathVelocity( } const size_t closest_idx = closest_idx_opt.get(); - /* if current_state = GO, and current_pose is in front of stop_line, ignore planning. */ - bool is_over_pass_judge_line = - static_cast(static_cast(closest_idx) > pass_judge_line_idx); - if (static_cast(closest_idx) == pass_judge_line_idx) { - geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose; - is_over_pass_judge_line = planning_utils::isAheadOf(current_pose.pose, pass_judge_line); - } - if (is_go_out_ && is_over_pass_judge_line && !external_stop) { - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx).point.pose.position)); - return true; // no plan needed. + const bool is_over_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); + if (is_over_pass_judge_line) { + /* + in case ego could not stop exactly before the stop line, but with some overshoot, keep + detection within some margin and low velocity threshold + */ + const bool is_before_keep_detection_line = + util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); + if ( + is_before_keep_detection_line && std::fabs(planner_data_->current_velocity->twist.linear.x) < + planner_param_.keep_detection_vel_thr) { + RCLCPP_DEBUG( + logger_, + "over the pass judge line, but before keep detection line and low speed, " + "continue planning"); + // no return here, keep planning + } else if (is_go_out_ && !external_stop) { + RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx).point.pose.position)); + return true; // no plan needed. + } } /* get dynamic object */ @@ -201,22 +214,24 @@ bool IntersectionModule::modifyPathVelocity( if (!isActivated()) { constexpr double v = 0.0; is_go_out_ = false; + int stop_line_idx_stop = stop_line_idx; + int pass_judge_line_idx_stop = pass_judge_line_idx; if (planner_param_.use_stuck_stopline && is_stuck) { int stuck_stop_line_idx = -1; int stuck_pass_judge_line_idx = -1; if (util::generateStopLineBeforeIntersection( lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx, &stuck_pass_judge_line_idx, logger_.get_child("util"))) { - stop_line_idx = stuck_stop_line_idx; - pass_judge_line_idx = stuck_pass_judge_line_idx; + stop_line_idx_stop = stuck_stop_line_idx; + pass_judge_line_idx_stop = stuck_pass_judge_line_idx; } } - planning_utils::setVelocityFromIndex(stop_line_idx, v, path); + planning_utils::setVelocityFromIndex(stop_line_idx_stop, v, path); debug_data_.stop_required = true; debug_data_.stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, base_link2front, *path); - debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; - debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.pose; + planning_utils::getAheadPose(stop_line_idx_stop, base_link2front, *path); + debug_data_.stop_point_pose = path->points.at(stop_line_idx_stop).point.pose; + debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_stop).point.pose; /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; @@ -278,7 +293,7 @@ bool IntersectionModule::checkCollision( ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point), &closest_lanelet); - debug_data_.ego_lane_polygon = toGeomMsg(ego_poly); + debug_data_.ego_lane_polygon = toGeomPoly(ego_poly); /* extract target objects */ autoware_auto_perception_msgs::msg::PredictedObjects target_objects; @@ -388,12 +403,14 @@ bool IntersectionModule::checkCollision( polygon.outer().emplace_back(polygon.outer().front()); - debug_data_.candidate_collision_ego_lane_polygon = toGeomMsg(polygon); + 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 = toPredictedFootprintPolygon(object, *itr); + const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); debug_data_.candidate_collision_object_polygons.emplace_back( - toGeomMsg(footprint_polygon)); + toGeomPoly(footprint_polygon)); if (bg::intersects(polygon, footprint_polygon)) { collision_detected = true; break; @@ -449,6 +466,7 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( } polygon.outer().emplace_back(polygon.outer().front()); + bg::correct(polygon); return polygon; } @@ -523,7 +541,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const Polygon2d & stuck_vehicle_detect_area) const { - debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); for (const auto & object : objects_ptr->objects) { if (!isTargetStuckVehicleType(object)) { @@ -535,7 +553,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection( } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = toFootprintPolygon(object); + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); @@ -546,27 +564,6 @@ bool IntersectionModule::checkStuckVehicleInIntersection( return false; } -Polygon2d IntersectionModule::toFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const -{ - Polygon2d obj_footprint; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - obj_footprint = toBoostPoly(object.shape.footprint); - } else { - // cylinder type is treated as square-polygon - obj_footprint = - obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); - } - return obj_footprint; -} - -Polygon2d IntersectionModule::toPredictedFootprintPolygon( - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const geometry_msgs::msg::Pose & predicted_pose) const -{ - return obj2polygon(predicted_pose, object.shape.dimensions); -} - bool IntersectionModule::isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const { @@ -779,7 +776,7 @@ bool IntersectionModule::checkFrontVehicleDeceleration( predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - Polygon2d predicted_obj_footprint = toFootprintPolygon(predicted_object); + auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); debug_data_.predicted_obj_pose.position = stopping_point; debug_data_.predicted_obj_pose.orientation = diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index eb1de17673ae6..02f992743ed33 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -83,27 +83,27 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( debug_data_.detection_area = conflicting_areas; /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; - int judge_line_idx = -1; - int first_idx_inside_lane = -1; + util::StopLineIdx stop_line_idxs; const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, - private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, + 0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - if (stop_line_idx <= 0 || judge_line_idx <= 0) { - RCLCPP_DEBUG(logger_, "stop line or judge line is at path[0], ignore planning."); + const int stop_line_idx = stop_line_idxs.stop_line_idx; + if (stop_line_idx <= 0) { + RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; } debug_data_.virtual_wall_pose = planning_utils::getAheadPose( stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; + const int first_idx_inside_lane = stop_line_idxs.first_idx_inside_lane; if (first_idx_inside_lane != -1) { debug_data_.first_collision_point = path->points.at(first_idx_inside_lane).point.pose.position; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 1d3e703d5ae39..c821d1044d72c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -73,7 +73,7 @@ bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, return false; } -bool hasDuplicatedPoint( +bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx) { @@ -116,9 +116,10 @@ int getFirstPointInsidePolygons( bool generateStopLine( const int lane_id, const std::vector detection_areas, const std::shared_ptr & planner_data, const double stop_line_margin, + const double keep_detection_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, - int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger) + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, + StopLineIdx * stop_line_idxs, const rclcpp::Logger logger) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -132,7 +133,8 @@ bool generateStopLine( /* set parameters */ constexpr double interval = 0.2; - const int margin_idx_dist = std::ceil(stop_line_margin / interval); + const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); + const int keep_detection_line_margin_idx_dist = std::ceil(keep_detection_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); @@ -143,6 +145,10 @@ bool generateStopLine( return false; } + int first_idx_inside_lane = -1; + int pass_judge_line_idx = -1; + int stop_line_idx = -1; + int keep_detection_line_idx = -1; /* generate stop point */ // If a stop_line is defined in lanelet_map, use it. // else, generates a local stop_line with considering the lane conflicts. @@ -163,32 +169,45 @@ bool generateStopLine( const auto first_idx_inside_lane_opt = motion_utils::findNearestIndex(original_path->points, first_inside_point, 10.0, M_PI_4); if (first_idx_inside_lane_opt) { - *first_idx_inside_lane = first_idx_inside_lane_opt.get(); + first_idx_inside_lane = first_idx_inside_lane_opt.get(); } - if (*first_idx_inside_lane == 0) { + if (first_idx_inside_lane == 0) { RCLCPP_DEBUG( logger, "path[0] is already in the detection area. This happens if you have " "already crossed the stop line or are very far from the intersection. Ignore computation."); - *stop_line_idx = 0; - *pass_judge_line_idx = 0; + stop_line_idxs->first_idx_inside_lane = 0; + stop_line_idxs->pass_judge_line_idx = 0; + stop_line_idxs->stop_line_idx = 0; + stop_line_idxs->keep_detection_line_idx = 0; return true; } - stop_idx_ip = std::max(first_idx_ip_inside_lane - 1 - margin_idx_dist - base2front_idx_dist, 0); + stop_idx_ip = + std::max(first_idx_ip_inside_lane - 1 - stop_line_margin_idx_dist - base2front_idx_dist, 0); + } + + /* insert keep_detection_line */ + const int keep_detection_idx_ip = std::min( + stop_idx_ip + keep_detection_line_margin_idx_dist, static_cast(path_ip.points.size()) - 1); + const auto inserted_keep_detection_point = path_ip.points.at(keep_detection_idx_ip).point.pose; + if (!util::getDuplicatedPointIdx( + *original_path, inserted_keep_detection_point.position, &keep_detection_line_idx)) { + keep_detection_line_idx = util::insertPoint(inserted_keep_detection_point, original_path); } /* insert stop_point */ const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as stop_line_idx - if (!util::hasDuplicatedPoint(*original_path, inserted_stop_point.position, stop_line_idx)) { - *stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + if (!util::getDuplicatedPointIdx(*original_path, inserted_stop_point.position, &stop_line_idx)) { + stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + ++keep_detection_line_idx; // the index is incremented by judge stop line insertion } /* if another stop point exist before intersection stop_line, disable judge_line. */ bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { + for (int i = 0; i < stop_line_idx; ++i) { if (std::fabs(original_path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { has_prior_stopline = true; break; @@ -199,23 +218,29 @@ bool generateStopLine( const int pass_judge_idx_ip = std::min( static_cast(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0)); if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { - *pass_judge_line_idx = *stop_line_idx; + pass_judge_line_idx = stop_line_idx; } else { const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( - *original_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { - *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); - ++(*stop_line_idx); // stop index is incremented by judge line insertion + if (!util::getDuplicatedPointIdx( + *original_path, inserted_pass_judge_point.position, &pass_judge_line_idx)) { + pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); + ++stop_line_idx; // stop index is incremented by judge line insertion + ++keep_detection_line_idx; // same. } } + stop_line_idxs->first_idx_inside_lane = first_idx_inside_lane; + stop_line_idxs->pass_judge_line_idx = pass_judge_line_idx; + stop_line_idxs->stop_line_idx = stop_line_idx; + stop_line_idxs->keep_detection_line_idx = keep_detection_line_idx; + RCLCPP_DEBUG( logger, "generateStopLine() : stop_idx = %d, pass_judge_idx = %d, stop_idx_ip = " "%d, pass_judge_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, *pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); + stop_line_idx, pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); return true; } @@ -507,7 +532,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; // if path has too close (= duplicated) point to the stop point, do not insert it // and consider the index of the duplicated point as *stuck_stop_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_stop_point.position, stuck_stop_line_idx)) { *stuck_stop_line_idx = util::insertPoint(inserted_stop_point, output_path); } @@ -531,7 +556,7 @@ bool generateStopLineBeforeIntersection( const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; // if path has too close (= duplicated) point to the pass judge point, do not insert it // and consider the index of the duplicated point as pass_judge_line_idx - if (!util::hasDuplicatedPoint( + if (!util::getDuplicatedPointIdx( *output_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, output_path); ++(*stuck_stop_line_idx); // stop index is incremented by judge line insertion @@ -556,5 +581,28 @@ geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) pose.position = p; return pose; } + +bool isOverTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(current_pose, target_pose); + } + return static_cast(closest_idx > target_idx); +} + +bool isBeforeTargetIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & current_pose, const int target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(target_pose, current_pose); + } + return static_cast(target_idx > closest_idx); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp index 27b96148d8747..ad8b526d9ab42 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp @@ -52,11 +52,14 @@ void NoStoppingAreaModuleManager::launchNewModules( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose)) { // Use lanelet_id to unregister module when the route is changed - const auto module_id = m.first->id(); + const int64_t module_id = m.first->id(); + const int64_t lane_id = m.second.id(); + if (!isModuleRegistered(module_id)) { // assign 1 no stopping area for each module registerModule(std::make_shared( - module_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), clock_)); + module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), + clock_)); generateUUID(module_id); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 5a0f83cac7b78..0472cc4aae720 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -40,10 +40,12 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; NoStoppingAreaModule::NoStoppingAreaModule( - const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param) { @@ -128,7 +130,7 @@ bool NoStoppingAreaModule::modifyPathVelocity( return true; } const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line.value(), planner_param_.stop_margin, + original_path, stop_line.value(), lane_id_, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { setSafe(true); @@ -160,8 +162,8 @@ bool NoStoppingAreaModule::modifyPathVelocity( setSafe(true); return true; } - debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area); - debug_data_.stop_line_detect_area = toGeomMsg(stop_line_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); + debug_data_.stop_line_detect_area = toGeomPoly(stop_line_detect_area); // Find stuck vehicle in no stopping area const bool is_entry_prohibited_by_stuck_vehicle = checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr); @@ -228,11 +230,11 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( continue; // not stop vehicle } // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object); + const Polygon2d obj_footprint = tier4_autoware_utils::toPolygon2d(object); const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); if (is_in_stuck_area) { RCLCPP_DEBUG(logger_, "stuck vehicle found."); - for (const auto p : obj_footprint.outer()) { + for (const auto & p : obj_footprint.outer()) { geometry_msgs::msg::Point point; point.x = p.x(); point.y = p.y(); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp index fa02d33fa0198..66d82418a0520 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp @@ -36,6 +36,8 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle)); // std::cout << boost::geometry::wkt(line_poly) << std::endl; // std::cout << boost::geometry::wkt(line) << std::endl; + + bg::correct(line_poly); return line_poly; } @@ -173,6 +175,8 @@ Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, cons poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, 0, 0).position)); poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position)); poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position)); + + bg::correct(poly); return poly; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 75404879f331d..3000fe6d1e042 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -61,8 +61,8 @@ PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0) } bool buildDetectionAreaPolygon( - Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose, - const PlannerParam & param) + Polygons2d & slices, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const PlannerParam & param) { const auto & p = param; DetectionRange da_range; @@ -76,7 +76,7 @@ bool buildDetectionAreaPolygon( da_range.max_lateral_distance = p.detection_area.max_lateral_distance; slices.clear(); return planning_utils::createDetectionAreaPolygons( - slices, path, pose, da_range, p.pedestrian_vel); + slices, path, target_pose, target_seg_idx, da_range, p.pedestrian_vel); } void calcSlowDownPointsForPossibleCollision( @@ -209,9 +209,9 @@ void categorizeVehicles( stuck_vehicle_foot_prints.clear(); for (const auto & vehicle : vehicles) { if (isMovingVehicle(vehicle, stuck_vehicle_vel)) { - moving_vehicle_foot_prints.emplace_back(planning_utils::toFootprintPolygon(vehicle)); + moving_vehicle_foot_prints.emplace_back(tier4_autoware_utils::toPolygon2d(vehicle)); } else if (isStuckVehicle(vehicle, stuck_vehicle_vel)) { - stuck_vehicle_foot_prints.emplace_back(planning_utils::toFootprintPolygon(vehicle)); + stuck_vehicle_foot_prints.emplace_back(tier4_autoware_utils::toPolygon2d(vehicle)); } } return; @@ -219,7 +219,7 @@ void categorizeVehicles( ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string) { - Polygon2d poly = planning_utils::toFootprintPolygon(obj); + const auto poly = tier4_autoware_utils::toPolygon2d(obj); std::deque arcs; for (const auto & p : poly.outer()) { lanelet::BasicPoint2d obj_p = {p.x(), p.y()}; @@ -342,7 +342,7 @@ std::vector filterVehiclesByDetectionArea( // stuck points by predicted objects for (const auto & object : objs) { // check if the footprint is in the stuck detect area - const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object); + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); for (const auto & p : polys) { if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index c4df98bb9a0b0..c9c062110c1d1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -122,8 +122,9 @@ bool OcclusionSpotModule::modifyPathVelocity( } } DEBUG_PRINT(show_time, "apply velocity [ms]: ", stop_watch_.toc("processing_time", true)); + const size_t ego_seg_idx = findEgoSegmentIndex(predicted_path.points); if (!utils::buildDetectionAreaPolygon( - debug_data_.detection_area_polygons, predicted_path, ego_pose, param_)) { + debug_data_.detection_area_polygons, predicted_path, ego_pose, ego_seg_idx, param_)) { return true; // path point is not enough } DEBUG_PRINT(show_time, "generate poly[ms]: ", stop_watch_.toc("processing_time", true)); diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index d398bb42fdac4..9f629927036f8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -156,8 +156,9 @@ Polygons2d RunOutModule::createDetectionAreaPolygon(const PathWithLaneId & smoot da_range.min_lateral_distance = p.vehicle_param.width / 2.0; da_range.max_lateral_distance = obstacle_vel_mps * p.dynamic_obstacle.max_prediction_time; Polygons2d detection_area_poly; + const size_t ego_seg_idx = findEgoSegmentIndex(smoothed_path.points); planning_utils::createDetectionAreaPolygons( - detection_area_poly, smoothed_path, planner_data_->current_pose.pose, da_range, + detection_area_poly, smoothed_path, planner_data_->current_pose.pose, ego_seg_idx, da_range, p.dynamic_obstacle.max_vel_kmph / 3.6); for (const auto & poly : detection_area_poly) { @@ -411,8 +412,8 @@ boost::optional RunOutModule::calcPredictedObstaclePos } bool RunOutModule::checkCollisionWithShape( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const Shape & shape, std::vector & collision_points) const + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const Shape & shape, + std::vector & collision_points) const { bool collision_detected = false; switch (shape.type) { @@ -438,8 +439,8 @@ bool RunOutModule::checkCollisionWithShape( } bool RunOutModule::checkCollisionWithCylinder( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, - const float radius, std::vector & collision_points) const + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const float radius, + std::vector & collision_points) const { // create bounding box for min and max velocity point const auto bounding_box_for_points = @@ -503,7 +504,7 @@ std::vector RunOutModule::createBoundingBoxForRangedP } bool RunOutModule::checkCollisionWithBoundingBox( - const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, const geometry_msgs::msg::Vector3 & dimension, std::vector & collision_points) const { diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp index 2c035705afd05..0f6921691025f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp @@ -195,6 +195,7 @@ Polygon2d createBoostPolyFromMsg(const std::vector & const auto & first_point = input_poly.front(); bg_poly.outer().emplace_back(bg::make(first_point.x, first_point.y)); + bg::correct(bg_poly); return bg_poly; } diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp index 1b847cf1e73f1..72daa9012a8b7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp @@ -103,7 +103,7 @@ visualization_msgs::msg::MarkerArray StopLineModule::createVirtualWallMarkerArra } const auto p_front = tier4_autoware_utils::calcOffsetPose( *debug_data_.stop_pose, debug_data_.base_link2front, 0.0, 0.0); - if (state_ == State::APPROACH) { + if (state_ == State::APPROACH || state_ == State::STOPPED) { appendMarkerArray( motion_utils::createStopVirtualWallMarker(p_front, "stopline", now, module_id_), &wall_marker, now); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp index aaf85b5db1f5e..4129494ef8703 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp @@ -29,7 +29,7 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) const std::string ns(getModuleName()); auto & p = planner_param_; p.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0); - p.stop_check_dist = node.declare_parameter(ns + ".stop_check_dist", 2.0); + p.hold_stop_margin_distance = node.declare_parameter(ns + ".hold_stop_margin_distance", 2.0); p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0); p.use_initialization_stop_line_state = node.declare_parameter(ns + ".use_initialization_stop_line_state", false); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index 3cd08f1f5b6e2..d547e13552dd8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -23,184 +24,18 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -double calcYawFromPoints( - const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back) -{ - return std::atan2(p_back.y - p_front.y, p_back.x - p_front.x); -} - -geometry_msgs::msg::Pose calcInterpolatedPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithOffset & offset_segment) -{ - // Get segment points - const auto & p_front = path.points.at(offset_segment.index).point.pose.position; - const auto & p_back = path.points.at(offset_segment.index + 1).point.pose.position; - - // To Eigen point - const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); - const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); - - // Calculate interpolation ratio - const auto interpolate_ratio = offset_segment.offset / (p_eigen_back - p_eigen_front).norm(); - - // Add offset to front point - const auto interpolated_point_2d = - p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); - const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); - - // Calculate orientation so that X-axis would be along the trajectory - tf2::Quaternion quat; - quat.setRPY(0, 0, calcYawFromPoints(p_front, p_back)); - - // To Pose - geometry_msgs::msg::Pose interpolated_pose; - interpolated_pose.position.x = interpolated_point_2d.x(); - interpolated_pose.position.y = interpolated_point_2d.y(); - interpolated_pose.position.z = interpolated_z; - interpolated_pose.orientation = tf2::toMsg(quat); - - return interpolated_pose; -} - -boost::optional findBackwardOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, - const double offset_length) -{ - double sum_length = 0.0; - const auto start = static_cast(base_idx) - 1; - for (std::int32_t i = start; i >= 0; --i) { - const auto p_front = to_bg2d(path.points.at(i).point.pose.position); - const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position); - - sum_length += bg::distance(p_front, p_back); - - // If it's over offset point, return front index and remain offset length - if (sum_length >= offset_length) { - const auto k = static_cast(i); - return StopLineModule::SegmentIndexWithOffset{k, sum_length - offset_length}; - } - } - - // No enough path length - return {}; -} - -} // namespace - StopLineModule::StopLineModule( const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - module_id_(module_id), - stop_line_(stop_line), lane_id_(lane_id), + stop_line_(stop_line), state_(State::APPROACH) { planner_param_ = planner_param; } -boost::optional StopLineModule::findCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const SearchRangeIndex & search_index) -{ - const size_t min_search_index = std::max(static_cast(0), search_index.min_idx); - const size_t max_search_index = std::min(search_index.max_idx, path.points.size() - 1); - - // for creating debug marker - if (planner_param_.show_stopline_collision_check) { - debug_data_.search_stopline = stop_line; - for (size_t i = min_search_index; i < max_search_index; ++i) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - debug_data_.search_segments.push_back(path_segment); - } - } - - for (size_t i = min_search_index; i < max_search_index; ++i) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - - // Find intersection - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(stop_line, path_segment, collision_points); - - // Ignore if no collision found - if (collision_points.empty()) { - continue; - } - - // Select first collision - const auto & collision_point = collision_points.at(0); - - return StopLineModule::SegmentIndexWithPoint2d{i, collision_point}; - } - - return {}; -} - -boost::optional StopLineModule::findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPoint2d & collision) -{ - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto base_backward_length = planner_param_.stop_margin + base_link2front; - - const auto & p_back = to_bg2d(path.points.at(collision.index + 1).point.pose.position); - - return findBackwardOffsetSegment( - path, collision.index + 1, base_backward_length + bg::distance(p_back, collision.point)); -} - -boost::optional StopLineModule::calcStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const boost::optional & offset_segment) -{ - // If no stop point found due to out of range, use front point on path - if (!offset_segment) { - return StopLineModule::SegmentIndexWithPose{0, path.points.front().point.pose}; - } - - return StopLineModule::SegmentIndexWithPose{ - offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; -} - -autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const StopLineModule::SegmentIndexWithPose & stop_pose_with_index, - tier4_planning_msgs::msg::StopReason * stop_reason) -{ - auto modified_path = path; - - // Insert stop pose to between segment start and end - size_t insert_index = static_cast(stop_pose_with_index.index + 1); - auto stop_point_with_lane_id = modified_path.points.at(insert_index); - stop_point_with_lane_id.point.pose = stop_pose_with_index.pose; - stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - - // Update first stop index - first_stop_path_point_index_ = static_cast(insert_index); - debug_data_.stop_pose = stop_point_with_lane_id.point.pose; - - // Insert stop point or replace with zero velocity - planning_utils::insertVelocity(modified_path, stop_point_with_lane_id, 0.0, insert_index); - - // Get stop point and stop factor - { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_point_with_lane_id.point.pose; - stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); - planning_utils::appendStopReason(stop_factor, stop_reason); - } - - return modified_path; -} - bool StopLineModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason) @@ -215,74 +50,115 @@ bool StopLineModule::modifyPathVelocity( const LineString2d stop_line = planning_utils::extendLine( stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); - const auto & current_position = planner_data_->current_pose.pose.position; - const PointWithSearchRangeIndex src_point_with_search_range_index = - planning_utils::findFirstNearSearchRangeIndex(path->points, current_position); - SearchRangeIndex dst_search_range = - planning_utils::getPathIndexRangeIncludeLaneId(*path, lane_id_); - // extend following and previous search range to avoid no collision - if (dst_search_range.max_idx < path->points.size() - 1) dst_search_range.max_idx++; - if (dst_search_range.min_idx > 0) dst_search_range.min_idx--; - - // Find collision - const auto collision = findCollision(*path, stop_line, dst_search_range); + // Calculate stop pose and insert index + const auto stop_point = arc_lane_utils::createTargetPoint( + *path, stop_line, lane_id_, planner_param_.stop_margin, + planner_data_->vehicle_info_.max_longitudinal_offset_m); // If no collision found, do nothing - if (!collision) { + if (!stop_point) { RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision"); return true; } - const double center_line_z = (stop_line_[0].z() + stop_line_[1].z()) / 2.0; - const auto stop_line_position = planning_utils::toRosPoint(collision->point, center_line_z); - // Find offset segment - const auto offset_segment = findOffsetSegment(*path, *collision); + const auto stop_point_idx = stop_point->first; + auto stop_pose = stop_point->second; + stop_pose.position.z = (stop_line_[0].z() + stop_line_[1].z()) / 2.0; - // Calculate stop pose and insert index - const auto stop_pose_with_index = calcStopPose(*path, offset_segment); - - const PointWithSearchRangeIndex dst_point_with_search_range_index = { - stop_line_position, dst_search_range}; - const double stop_line_margin = base_link2front + planner_param_.stop_margin; /** * @brief : calculate signed arc length consider stop margin from stop line * * |----------------------------| * s---ego----------x--|--------g */ - const double signed_arc_dist_to_stop_point = - planning_utils::calcSignedArcLengthWithSearchIndex( - path->points, src_point_with_search_range_index, dst_point_with_search_range_index) - - stop_line_margin; - if (state_ == State::APPROACH) { - // Insert stop pose - *path = insertStopPose(*path, *stop_pose_with_index, stop_reason); + const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, stop_pose.position, stop_point_idx); + const size_t current_seg_idx = findEgoSegmentIndex(path->points); + const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, current_seg_idx, stop_pose.position, + stop_line_seg_idx); + switch (state_) { + case State::APPROACH: { + // Insert stop pose + planning_utils::insertStopPoint(stop_pose.position, stop_line_seg_idx, *path); + + // Update first stop index + first_stop_path_point_index_ = static_cast(stop_point_idx); + debug_data_.stop_pose = stop_pose; + + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); + planning_utils::appendStopReason(stop_factor, stop_reason); + } + + // Move to stopped state if stopped + if ( + signed_arc_dist_to_stop_point < planner_param_.hold_stop_margin_distance && + planner_data_->isVehicleStopped()) { + RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + + state_ = State::STOPPED; + stopped_time_ = std::make_shared(clock_->now()); - // Move to stopped state if stopped - if ( - signed_arc_dist_to_stop_point < planner_param_.stop_check_dist && - planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { - RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); - state_ = State::STOPPED; - if (signed_arc_dist_to_stop_point < -planner_param_.stop_check_dist) { - RCLCPP_ERROR( - logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED"); + if (signed_arc_dist_to_stop_point < -planner_param_.hold_stop_margin_distance) { + RCLCPP_ERROR( + logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED"); + } } + + break; } - } else if (state_ == State::STOPPED) { - // Change state after vehicle departure - if (!planner_data_->isVehicleStopped()) { - RCLCPP_INFO(logger_, "STOPPED -> START"); - state_ = State::START; + + case State::STOPPED: { + // Change state after vehicle departure + const auto stopped_pose = motion_utils::calcLongitudinalOffsetPose( + path->points, planner_data_->current_pose.pose.position, 0.0); + + if (!stopped_pose) { + break; + } + + SegmentIndexWithPose ego_pos_on_path; + ego_pos_on_path.pose = stopped_pose.get(); + ego_pos_on_path.index = findEgoSegmentIndex(path->points); + + // Insert stop pose + planning_utils::insertStopPoint(ego_pos_on_path.pose.position, ego_pos_on_path.index, *path); + + debug_data_.stop_pose = stop_pose; + + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = ego_pos_on_path.pose; + stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); + planning_utils::appendStopReason(stop_factor, stop_reason); + } + + const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); + + if (planner_param_.stop_duration_sec < elapsed_time) { + RCLCPP_INFO(logger_, "STOPPED -> START"); + state_ = State::START; + } + + break; } - } else if (state_ == State::START) { - // Initialize if vehicle is far from stop_line - if (planner_param_.use_initialization_stop_line_state) { - if (signed_arc_dist_to_stop_point > planner_param_.stop_check_dist) { - RCLCPP_INFO(logger_, "START -> APPROACH"); - state_ = State::APPROACH; + + case State::START: { + // Initialize if vehicle is far from stop_line + if (planner_param_.use_initialization_stop_line_state) { + if (signed_arc_dist_to_stop_point > planner_param_.hold_stop_margin_distance) { + RCLCPP_INFO(logger_, "START -> APPROACH"); + state_ = State::APPROACH; + } } + + break; } } diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index d00cd239b8c71..c5fb9956ff18b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -108,11 +108,12 @@ void TrafficLightModuleManager::launchNewModules( } // Use lanelet_id to unregister module when the route is changed - const auto module_id = traffic_light_reg_elem.second.id(); + const auto lane_id = traffic_light_reg_elem.second.id(); + const auto module_id = lane_id; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, - logger_.get_child("traffic_light_module"), clock_)); + module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, + planner_param_, logger_.get_child("traffic_light_module"), clock_)); generateUUID(module_id); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 516704d47be22..ed5ba00e18899 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -187,10 +187,12 @@ autoware_auto_perception_msgs::msg::LookingTrafficSignal initializeTrafficSignal } // namespace TrafficLightModule::TrafficLightModule( - const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), state_(State::APPROACH), diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index f509e8d5e0133..f8ae83dc6f9a7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -50,10 +50,11 @@ void VirtualTrafficLightModuleManager::launchNewModules( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_pose.pose)) { // Use lanelet_id to unregister module when the route is changed - const auto module_id = m.second.id(); + const auto lane_id = m.second.id(); + const auto module_id = lane_id; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *m.first, m.second, planner_param_, + module_id, lane_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_)); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index f8bad074bc1ab..af88095a97085 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -76,7 +76,8 @@ std::vector toAutowarePoints( return output; } -tier4_autoware_utils::LineString2d to_2d(const tier4_autoware_utils::LineString3d & line_string) +[[maybe_unused]] tier4_autoware_utils::LineString2d to_2d( + const tier4_autoware_utils::LineString3d & line_string) { tier4_autoware_utils::LineString2d output; for (const auto & p : line_string) { @@ -99,52 +100,45 @@ geometry_msgs::msg::Pose calcHeadPose( return tier4_autoware_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); } -template -boost::optional findCollision( - const T & points, const tier4_autoware_utils::LineString3d & line) +geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point3d & p) { - for (size_t i = 0; i < points.size() - 1; ++i) { - // Create path segment - const auto & p_front = tier4_autoware_utils::getPoint(points.at(i)); - const auto & p_back = tier4_autoware_utils::getPoint(points.at(i + 1)); - const tier4_autoware_utils::LineString2d path_segment{ - {p_front.x, p_front.y}, {p_back.x, p_back.y}}; - - // Find intersection - std::vector collision_points; - bg::intersection(to_2d(line), path_segment, collision_points); - - // Ignore if no collision found - if (collision_points.empty()) { - continue; - } - - // Select first collision if found - const auto collision_point_2d = collision_points.front(); + geometry_msgs::msg::Point geom_p; + geom_p.x = p.x(); + geom_p.y = p.y(); - // Calculate interpolation ratio - const auto interpolate_ratio = - calcDistance2d(p_front, toMsg(collision_point_2d.to_3d(0))) / calcDistance2d(p_front, p_back); - - // Interpolate z - const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); - - // To point - const auto collision_point = tier4_autoware_utils::createPoint( - collision_point_2d.x(), collision_point_2d.y(), interpolated_z); + return geom_p; +} - return SegmentIndexWithPoint{i, collision_point}; +template +boost::optional findLastCollisionBeforeEndLine( + const T & points, const tier4_autoware_utils::LineString3d & target_line, + const size_t end_line_idx) +{ + const auto target_line_p1 = convertToGeomPoint(target_line.at(0)); + const auto target_line_p2 = convertToGeomPoint(target_line.at(1)); + + for (size_t i = end_line_idx; 0 < i; + --i) { // NOTE: size_t can be used since it will not be negative. + const auto & p1 = tier4_autoware_utils::getPoint(points.at(i)); + const auto & p2 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto collision_point = + arc_lane_utils::checkCollision(p1, p2, target_line_p1, target_line_p2); + + if (collision_point) { + return SegmentIndexWithPoint{i, collision_point.get()}; + } } return {}; } template -boost::optional findCollision( - const T & points, const std::vector & lines) +boost::optional findLastCollisionBeforeEndLine( + const T & points, const std::vector & lines, + const size_t end_line_idx) { for (const auto & line : lines) { - const auto collision = findCollision(points, line); + const auto collision = findLastCollisionBeforeEndLine(points, line, end_line_idx); if (collision) { return collision; } @@ -190,11 +184,12 @@ boost::optional insertStopVelocityAtCollision( } // namespace VirtualTrafficLightModule::VirtualTrafficLightModule( - const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, - lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t module_id, const int64_t lane_id, + const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, + const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - module_id_(module_id), + lane_id_(lane_id), reg_elem_(reg_elem), lane_(lane), planner_param_(planner_param) @@ -264,8 +259,17 @@ bool VirtualTrafficLightModule::modifyPathVelocity( planner_data_->current_pose.pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); module_data_.path = *path; + // Calculate path index of end line + // NOTE: In order to deal with u-turn or self-crossing path, only start/stop lines before the end + // line are used when whether the ego is before/after the start/stop/end lines is calculated. + const auto opt_end_line_idx = getPathIndexOfFirstEndLine(); + if (!opt_end_line_idx) { + return true; + } + const size_t end_line_idx = opt_end_line_idx.get(); + // Do nothing if vehicle is before start line - if (isBeforeStartLine()) { + if (isBeforeStartLine(end_line_idx)) { RCLCPP_DEBUG(logger_, "before start_line"); state_ = State::NONE; updateInfrastructureCommand(); @@ -273,7 +277,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity( } // Do nothing if vehicle is after any end line - if (isAfterAnyEndLine() || state_ == State::FINALIZED) { + if (isAfterAnyEndLine(end_line_idx) || state_ == State::FINALIZED) { RCLCPP_DEBUG(logger_, "after end_line"); state_ = State::FINALIZED; updateInfrastructureCommand(); @@ -295,7 +299,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity( // Stop at stop_line if no message received if (!virtual_traffic_light_state) { RCLCPP_DEBUG(logger_, "no message received"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); updateInfrastructureCommand(); return true; } @@ -303,16 +307,16 @@ bool VirtualTrafficLightModule::modifyPathVelocity( // Stop at stop_line if no right is given if (!hasRightOfWay(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "no right is given"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); updateInfrastructureCommand(); return true; } // Stop at stop_line if state is timeout before stop_line - if (isBeforeStopLine()) { + if (isBeforeStopLine(end_line_idx)) { if (isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout before stop line"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); } updateInfrastructureCommand(); @@ -326,7 +330,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity( if ( planner_param_.check_timeout_after_stop_line && isStateTimeout(*virtual_traffic_light_state)) { RCLCPP_DEBUG(logger_, "state is timeout after stop line"); - insertStopVelocityAtStopLine(path, stop_reason); + insertStopVelocityAtStopLine(path, stop_reason, end_line_idx); updateInfrastructureCommand(); return true; } @@ -334,9 +338,9 @@ bool VirtualTrafficLightModule::modifyPathVelocity( // Stop at stop_line if finalization isn't completed if (!virtual_traffic_light_state->is_finalized) { RCLCPP_DEBUG(logger_, "finalization isn't completed"); - insertStopVelocityAtEndLine(path, stop_reason); + insertStopVelocityAtEndLine(path, stop_reason, end_line_idx); - if (isNearAnyEndLine() && planner_data_->isVehicleStopped()) { + if (isNearAnyEndLine(end_line_idx) && planner_data_->isVehicleStopped()) { state_ = State::FINALIZING; } } @@ -361,79 +365,128 @@ void VirtualTrafficLightModule::setStopReason( planning_utils::appendStopReason(stop_factor, stop_reason); } -bool VirtualTrafficLightModule::isBeforeStartLine() +boost::optional VirtualTrafficLightModule::getPathIndexOfFirstEndLine() { - const auto collision = findCollision(module_data_.path.points, map_data_.start_line); + boost::optional min_seg_idx; + for (const auto & end_line : map_data_.end_lines) { + geometry_msgs::msg::Point end_line_p1; + end_line_p1.x = end_line.front().x(); + end_line_p1.y = end_line.front().y(); + + geometry_msgs::msg::Point end_line_p2; + end_line_p2.x = end_line.back().x(); + end_line_p2.y = end_line.back().y(); + + const auto collision = + arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, lane_id_); + if (!collision) { + continue; + } + + const size_t collision_seg_idx = collision->first; + + if (!min_seg_idx || collision_seg_idx < min_seg_idx.get()) { + min_seg_idx = + collision_seg_idx + 1; // NOTE: In order that min_seg_idx will be after the end line + } + } + + return min_seg_idx; +} + +bool VirtualTrafficLightModule::isBeforeStartLine(const size_t end_line_idx) +{ + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, map_data_.start_line, end_line_idx); // Since the module is registered, a collision should be detected usually. // Therefore if no collision found, vehicle's path is fully after the line. if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return *signed_arc_length > 0; + return signed_arc_length > 0; } -bool VirtualTrafficLightModule::isBeforeStopLine() +bool VirtualTrafficLightModule::isBeforeStopLine(const size_t end_line_idx) { - const auto collision = findCollision(module_data_.path.points, *map_data_.stop_line); + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, *map_data_.stop_line, end_line_idx); // Since the module is registered, a collision should be detected usually. // Therefore if no collision found, vehicle's path is fully after the line. if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return *signed_arc_length > -planner_param_.dead_line_margin; + return signed_arc_length > -planner_param_.dead_line_margin; } -bool VirtualTrafficLightModule::isAfterAnyEndLine() +bool VirtualTrafficLightModule::isAfterAnyEndLine(const size_t end_line_idx) { // Assume stop line is before end lines - if (isBeforeStopLine()) { + if (isBeforeStopLine(end_line_idx)) { return false; } - const auto collision = findCollision(module_data_.path.points, map_data_.end_lines); + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, map_data_.end_lines, end_line_idx); // If the goal is set before the end line, collision will not be detected. // Therefore if there is no collision, the ego vehicle is assumed to be before the end line. if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return *signed_arc_length < -planner_param_.dead_line_margin; + return signed_arc_length < -planner_param_.dead_line_margin; } -bool VirtualTrafficLightModule::isNearAnyEndLine() +bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) { - const auto collision = findCollision(module_data_.path.points, map_data_.end_lines); + const auto collision = + findLastCollisionBeforeEndLine(module_data_.path.points, map_data_.end_lines, end_line_idx); if (!collision) { return false; } + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + module_data_.path.points, collision->point, collision->index); - const double max_dist = std::numeric_limits::max(); + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(module_data_.path.points); const auto signed_arc_length = motion_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose, collision->point, max_dist, - planner_param_.max_yaw_deviation_rad); + module_data_.path.points, ego_pose.position, ego_seg_idx, + collision->point, collision_seg_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; - return std::abs(*signed_arc_length) < planner_param_.near_line_distance; + return std::abs(signed_arc_length) < planner_param_.near_line_distance; } boost::optional @@ -473,9 +526,10 @@ bool VirtualTrafficLightModule::hasRightOfWay( void VirtualTrafficLightModule::insertStopVelocityAtStopLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { - const auto collision = findCollision(path->points, *map_data_.stop_line); + const auto collision = + findLastCollisionBeforeEndLine(path->points, *map_data_.stop_line, end_line_idx); const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m; geometry_msgs::msg::Pose stop_pose{}; @@ -483,19 +537,25 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( insertStopVelocityFromStart(path); stop_pose = planner_data_->current_pose.pose; } else { - const auto & ego_pos = planner_data_->current_pose.pose.position; + const auto & ego_pose = planner_data_->current_pose.pose; + const size_t ego_seg_idx = findEgoSegmentIndex(path->points); + const size_t collision_seg_idx = planning_utils::calcSegmentIndexFromPointIndex( + path->points, collision->point, collision->index); + const auto stop_distance = - motion_utils::calcSignedArcLength(path->points, ego_pos, collision.get().point) + offset; + motion_utils::calcSignedArcLength( + path->points, ego_pose.position, ego_seg_idx, collision.get().point, collision_seg_idx) + + offset; const auto is_stopped = planner_data_->isVehicleStopped(); if (stop_distance < planner_param_.hold_stop_margin_distance && is_stopped) { SegmentIndexWithPoint new_collision; const auto ego_pos_on_path = - motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pos, 0.0); + motion_utils::calcLongitudinalOffsetPoint(path->points, ego_pose.position, 0.0); if (ego_pos_on_path) { new_collision.point = ego_pos_on_path.get(); - new_collision.index = motion_utils::findNearestSegmentIndex(path->points, ego_pos); + new_collision.index = ego_seg_idx; insertStopVelocityAtCollision(new_collision, 0.0, path); } @@ -526,14 +586,15 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( void VirtualTrafficLightModule::insertStopVelocityAtEndLine( autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) + tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { - const auto collision = findCollision(path->points, map_data_.end_lines); + const auto collision = + findLastCollisionBeforeEndLine(path->points, map_data_.end_lines, end_line_idx); geometry_msgs::msg::Pose stop_pose{}; if (!collision) { // No enough length - if (isBeforeStopLine()) { + if (isBeforeStopLine(end_line_idx)) { return; } diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 0256c4ba09d49..9179a53ab854e 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -47,8 +47,9 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con } bool createDetectionAreaPolygons( - Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & pose, - const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity) + Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, + const double min_velocity) { /** * @brief relationships for interpolated polygon @@ -67,28 +68,27 @@ bool createDetectionAreaPolygons( const size_t max_index = static_cast(path.points.size() - 1); //! avoid bug with same point polygon const double eps = 1e-3; - auto nearest_idx = findNearestIndex(path.points, pose.position); + auto nearest_idx = + calcPointIndexFromSegmentIndex(path.points, target_pose.position, target_seg_idx); if (max_index == nearest_idx) return false; // case of path point is not enough size auto p0 = path.points.at(nearest_idx).point; auto first_idx = nearest_idx + 1; // use ego point as start point if same point as ego is not in the path const auto dist_to_nearest = - std::fabs(calcSignedArcLength(path.points, pose.position, nearest_idx)); + std::fabs(calcSignedArcLength(path.points, target_pose.position, target_seg_idx, nearest_idx)); if (dist_to_nearest > eps) { - const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, pose.position); - // interpolate ego point const auto & pp = path.points; - const double ds = calcDistance2d(pp.at(nearest_seg_idx), pp.at(nearest_seg_idx + 1)); - const double dist_to_nearest_seg = - calcSignedArcLength(path.points, nearest_seg_idx, pose.position); - const double ratio = dist_to_nearest_seg / ds; + const double ds = calcDistance2d(pp.at(target_seg_idx), pp.at(target_seg_idx + 1)); + const double dist_to_target_seg = + calcSignedArcLength(path.points, target_seg_idx, target_pose.position, target_seg_idx); + const double ratio = dist_to_target_seg / ds; p0 = getLerpPathPointWithLaneId( - pp.at(nearest_seg_idx).point, pp.at(nearest_seg_idx + 1).point, ratio); + pp.at(target_seg_idx).point, pp.at(target_seg_idx + 1).point, ratio); // new first index should be ahead of p0 - first_idx = nearest_seg_idx + 1; + first_idx = target_seg_idx + 1; } double ttc = 0.0; @@ -212,7 +212,7 @@ void insertVelocity( std::min(static_cast(insert_index + 1), static_cast(path.points.size() - 1)); for (int i = min_idx; i <= max_idx; i++) { if (calcDistance2d(path.points.at(static_cast(i)), path_point) < min_distance) { - path.points.at(i).point.longitudinal_velocity_mps = 0; + path.points.at(i).point.longitudinal_velocity_mps = v; already_has_path_point = true; insert_index = static_cast(i); // set velocity from is going to insert min velocity later @@ -227,19 +227,6 @@ void insertVelocity( setVelocityFromIndex(insert_index, v, &path); } -Polygon2d toFootprintPolygon(const PredictedObject & object) -{ - Polygon2d obj_footprint; - if (object.shape.type == Shape::POLYGON) { - obj_footprint = toBoostPoly(object.shape.footprint); - } else { - // cylinder type is treated as square-polygon - obj_footprint = - obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); - } - return obj_footprint; -} - bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) { geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); @@ -296,6 +283,8 @@ Polygon2d generatePathPolygon( const double y = path.points.at(i).point.pose.position.y + width * std::cos(yaw); ego_area.outer().push_back(Point2d(x, y)); } + + bg::correct(ego_area); return ego_area; } @@ -523,15 +512,8 @@ LineString2d extendLine( boost::optional getNearestLaneId( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, - const geometry_msgs::msg::Pose & current_pose, boost::optional & nearest_segment_idx) + const geometry_msgs::msg::Pose & current_pose) { - nearest_segment_idx = motion_utils::findNearestSegmentIndex( - path.points, current_pose, std::numeric_limits::max(), M_PI_2); - - if (!nearest_segment_idx) { - return boost::none; - } - lanelet::ConstLanelets lanes; const auto lane_ids = getSortedLaneIdsFromPath(path); for (const auto & lane_id : lane_ids) { @@ -549,9 +531,7 @@ std::vector getLaneletsOnPath( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose) { - boost::optional nearest_segment_idx; - const auto nearest_lane_id = - getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx); + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { @@ -614,6 +594,7 @@ std::vector getSubsequentLaneIdsSetOnPath( return subsequent_lane_ids; } +// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, @@ -630,6 +611,7 @@ boost::optional insertDecelPoint( { // TODO(tanaka): consider proper overlap threshold for inserting decel point const double overlap_threshold = 5e-2; + // TODO(murooka): remove this function for u-turn and crossing-path const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point); const auto insert_idx = motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold); @@ -646,6 +628,7 @@ boost::optional insertDecelPoint( return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); } +// TODO(murooka): remove this function for u-turn and crossing-path boost::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output) { @@ -662,5 +645,21 @@ boost::optional insertStopPoint( return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); } + +boost::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output) +{ + const auto insert_idx = motion_utils::insertTargetPoint(stop_seg_idx, stop_point, output.points); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.get(); i < output.points.size(); ++i) { + output.points.at(i).point.longitudinal_velocity_mps = 0.0; + } + + return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); +} } // namespace planning_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp index d39b4ccb40d1b..ac5a5c82d6730 100644 --- a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp @@ -26,6 +26,18 @@ using LineString2d = behavior_velocity_planner::LineString2d; using Point2d = behavior_velocity_planner::Point2d; namespace arc_lane_utils = behavior_velocity_planner::arc_lane_utils; +namespace +{ +geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} +} // namespace + TEST(findCollisionSegment, nominal) { /** @@ -36,13 +48,17 @@ TEST(findCollisionSegment, nominal) * **/ auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + for (auto & point : path.points) { + point.lane_ids.push_back(100); + } + LineString2d stop_line; stop_line.emplace_back(Point2d(3.5, 3.0)); stop_line.emplace_back(Point2d(3.5, -3.0)); - auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, 100); EXPECT_EQ(segment->first, static_cast(3)); - EXPECT_DOUBLE_EQ(segment->second.x(), 3.5); - EXPECT_DOUBLE_EQ(segment->second.y(), 0.0); + EXPECT_DOUBLE_EQ(segment->second.x, 3.5); + EXPECT_DOUBLE_EQ(segment->second.y, 0.0); } TEST(findOffsetSegment, case_forward_offset_segment) @@ -54,9 +70,8 @@ TEST(findOffsetSegment, case_forward_offset_segment) * 0 1 2 3 4 5 **/ const int collision_segment_idx = 3; - Point2d collision_point(3.5, 0.0); - const PathIndexWithPoint2d & collision_segment = - std::make_pair(collision_segment_idx, collision_point); + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); // nominal { double offset_length = 1.0; @@ -84,9 +99,8 @@ TEST(findOffsetSegment, case_backward_offset_segment) * 0 1 2 3 4 5 **/ const int collision_segment_idx = 3; - Point2d collision_point(3.5, 0.0); - const PathIndexWithPoint2d & collision_segment = - std::make_pair(collision_segment_idx, collision_point); + const auto collision_point = createPoint(3.5, 0.0, 0.0); + const auto & collision_segment = std::make_pair(collision_segment_idx, collision_point); // nominal { double offset_length = -1.0; @@ -104,3 +118,127 @@ TEST(findOffsetSegment, case_backward_offset_segment) EXPECT_FALSE(offset_segment); } } + +TEST(checkCollision, various_cases) +{ + using behavior_velocity_planner::arc_lane_utils::checkCollision; + constexpr double epsilon = 1e-6; + + { // normal case with collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // normal case without collision + const auto p1 = createPoint(1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // normal case without collision + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 1.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // line and point + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(2.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // point and line + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(-1.0, 0.0, 0.0); + const auto p4 = createPoint(2.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_EQ(collision, boost::none); + } + + { // collision with edges + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 2.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, 0.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 0.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(0.0, 0.0, 0.0); + const auto p2 = createPoint(1.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } + + { // collision with edge + const auto p1 = createPoint(-1.0, 0.0, 0.0); + const auto p2 = createPoint(0.0, 0.0, 0.0); + const auto p3 = createPoint(0.0, -1.0, 0.0); + const auto p4 = createPoint(0.0, 1.0, 0.0); + + const auto collision = checkCollision(p1, p2, p3, p4); + EXPECT_NE(collision, boost::none); + EXPECT_NEAR(collision->x, 0.0, epsilon); + EXPECT_NEAR(collision->y, 0.0, epsilon); + EXPECT_NEAR(collision->z, 0.0, epsilon); + } +} diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp index 441d6b94bbc03..19e9b5f37b028 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp @@ -43,6 +43,7 @@ using behavior_velocity_planner::Point2d; using behavior_velocity_planner::Polygon2d; using behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; +namespace bg = boost::geometry; Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) { @@ -57,6 +58,8 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle)); // std::cout << boost::geometry::wkt(line_poly) << std::endl; // std::cout << boost::geometry::wkt(line) << std::endl; + + bg::correct(line_poly); return line_poly; } diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/test/src/test_utilization.cpp index 1a9d4cdbf4aed..742cccecb9773 100644 --- a/planning/behavior_velocity_planner/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner/test/src/test_utilization.cpp @@ -33,14 +33,6 @@ std::cerr << ss.str() << std::endl; \ } -TEST(to_footprint_polygon, nominal) -{ - using behavior_velocity_planner::planning_utils::toFootprintPolygon; - autoware_auto_perception_msgs::msg::PredictedObject obj = test::generatePredictedObject(0.0); - auto poly = toFootprintPolygon(obj); - EXPECT_TRUE(true); -} - TEST(is_ahead_of, nominal) { using behavior_velocity_planner::planning_utils::isAheadOf; diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index 070c160d3aa05..a65a5bba21000 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -10,6 +10,8 @@ BSD-3-Clause + Kenji Miyake + ament_cmake_auto autoware_cmake diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/external_velocity_limit_selector/package.xml index 85f2274025ac9..2a0d2ff51680c 100644 --- a/planning/external_velocity_limit_selector/package.xml +++ b/planning/external_velocity_limit_selector/package.xml @@ -7,6 +7,8 @@ Satoshi Ota Apache License 2.0 + Satoshi Ota + ament_cmake_auto autoware_cmake diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index 046d58510c4bc..614049e1e5222 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -4,12 +4,12 @@ freespace_planner 0.1.0 The freespace_planner package - Kenji Miyake - Akihito OHSATO + Takamasa Horibe Apache License 2.0 - Akihito OHSATO - Tomohito ANDO + Kenji Miyake + Akihito OHSATO + Tomohito ANDO ament_cmake_auto diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 94f7b9c3d8661..83bec1da0ab03 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -4,9 +4,11 @@ mission_planner 0.1.0 The mission_planner package - mitsudome-r + Ryohsuke Mitsudome Apache License 2.0 + Ryohsuke Mitsudome + ament_cmake_auto autoware_cmake diff --git a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp b/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp index 157ce13c0b75e..a5e70ffef6d88 100644 --- a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp +++ b/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp @@ -21,12 +21,12 @@ GoalPoseVisualizer::GoalPoseVisualizer(const rclcpp::NodeOptions & node_options) { sub_route_ = create_subscription( "input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&GoalPoseVisualizer::echoBackRouteCallback, this, std::placeholders::_1)); + std::bind(&GoalPoseVisualizer::echo_back_route_callback, this, std::placeholders::_1)); pub_goal_pose_ = create_publisher( "output/goal_pose", rclcpp::QoS{1}.transient_local()); } -void GoalPoseVisualizer::echoBackRouteCallback( +void GoalPoseVisualizer::echo_back_route_callback( const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg) { geometry_msgs::msg::PoseStamped goal_pose; diff --git a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp b/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp index 28af8a4cb976c..718a721239e33 100644 --- a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp +++ b/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp @@ -31,7 +31,7 @@ class GoalPoseVisualizer : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Publisher::SharedPtr pub_goal_pose_; - void echoBackRouteCallback( + void echo_back_route_callback( const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg); }; diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index e210fc901c56e..000d7ac6260e2 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -40,9 +40,9 @@ MissionPlanner::MissionPlanner( using std::placeholders::_1; goal_subscriber_ = create_subscription( - "input/goal_pose", 10, std::bind(&MissionPlanner::goalPoseCallback, this, _1)); + "input/goal_pose", 10, std::bind(&MissionPlanner::goal_pose_callback, this, _1)); checkpoint_subscriber_ = create_subscription( - "input/checkpoint", 10, std::bind(&MissionPlanner::checkpointCallback, this, _1)); + "input/checkpoint", 10, std::bind(&MissionPlanner::checkpoint_callback, this, _1)); rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); @@ -52,7 +52,7 @@ MissionPlanner::MissionPlanner( create_publisher("debug/route_marker", durable_qos); } -bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose) +bool MissionPlanner::get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose) { geometry_msgs::msg::PoseStamped base_link_origin; base_link_origin.header.frame_id = base_link_frame_; @@ -65,12 +65,12 @@ bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_veh base_link_origin.pose.orientation.w = 1; // transform base_link frame origin to map_frame to get vehicle positions - return transformPose(base_link_origin, ego_vehicle_pose, map_frame_); + return transform_pose(base_link_origin, ego_vehicle_pose, map_frame_); } -bool MissionPlanner::transformPose( +bool MissionPlanner::transform_pose( const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose, - const std::string target_frame) + const std::string & target_frame) { geometry_msgs::msg::TransformStamped transform; try { @@ -84,17 +84,17 @@ bool MissionPlanner::transformPose( } } -void MissionPlanner::goalPoseCallback( +void MissionPlanner::goal_pose_callback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr) { // set start pose - if (!getEgoVehiclePose(&start_pose_)) { + if (!get_ego_vehicle_pose(&start_pose_)) { RCLCPP_ERROR( get_logger(), "Failed to get ego vehicle pose in map frame. Aborting mission planning"); return; } // set goal pose - if (!transformPose(*goal_msg_ptr, &goal_pose_, map_frame_)) { + if (!transform_pose(*goal_msg_ptr, &goal_pose_, map_frame_)) { RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning"); return; } @@ -104,16 +104,16 @@ void MissionPlanner::goalPoseCallback( checkpoints_.push_back(start_pose_); checkpoints_.push_back(goal_pose_); - if (!isRoutingGraphReady()) { + if (!is_routing_graph_ready()) { RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning"); return; } - autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(); - publishRoute(route); + autoware_auto_planning_msgs::msg::HADMapRoute route = plan_route(); + publish_route(route); } // namespace mission_planner -void MissionPlanner::checkpointCallback( +void MissionPlanner::checkpoint_callback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr) { if (checkpoints_.size() < 2) { @@ -124,7 +124,7 @@ void MissionPlanner::checkpointCallback( } geometry_msgs::msg::PoseStamped transformed_checkpoint; - if (!transformPose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) { + if (!transform_pose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) { RCLCPP_ERROR( get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning"); return; @@ -133,16 +133,17 @@ void MissionPlanner::checkpointCallback( // insert checkpoint before goal checkpoints_.insert(checkpoints_.end() - 1, transformed_checkpoint); - autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(); - publishRoute(route); + autoware_auto_planning_msgs::msg::HADMapRoute route = plan_route(); + publish_route(route); } -void MissionPlanner::publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const +void MissionPlanner::publish_route( + const autoware_auto_planning_msgs::msg::HADMapRoute & route) const { if (!route.segments.empty()) { RCLCPP_INFO(get_logger(), "Route successfully planned. Publishing..."); route_publisher_->publish(route); - visualizeRoute(route); + visualize_route(route); } else { RCLCPP_ERROR(get_logger(), "Calculated route is empty!"); } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 9ab1860c5f6f7..066a5549dbe69 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -47,11 +47,11 @@ class MissionPlanner : public rclcpp::Node rclcpp::Publisher::SharedPtr marker_publisher_; - virtual bool isRoutingGraphReady() const = 0; - virtual autoware_auto_planning_msgs::msg::HADMapRoute planRoute() = 0; - virtual void visualizeRoute( + virtual bool is_routing_graph_ready() const = 0; + virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route() = 0; + virtual void visualize_route( const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0; - virtual void publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const; + virtual void publish_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const; private: rclcpp::Publisher::SharedPtr route_publisher_; @@ -61,12 +61,13 @@ class MissionPlanner : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - bool getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose); - void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); - void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); - bool transformPose( + bool get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose); + void goal_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); + void checkpoint_callback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); + bool transform_pose( const geometry_msgs::msg::PoseStamped & input_pose, - geometry_msgs::msg::PoseStamped * output_pose, const std::string target_frame); + geometry_msgs::msg::PoseStamped * output_pose, const std::string & target_frame); }; } // namespace mission_planner diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp index 8c94071306494..2249ec4a4750e 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp @@ -38,7 +38,7 @@ namespace { using RouteSections = std::vector; -RouteSections combineConsecutiveRouteSections( +RouteSections combine_consecutive_route_sections( const RouteSections & route_sections1, const RouteSections & route_sections2) { RouteSections route_sections; @@ -53,17 +53,16 @@ RouteSections combineConsecutiveRouteSections( return route_sections; } -double normalizeRadian(const double rad, const double min_rad = -M_PI, const double max_rad = M_PI) +double normalize_radian(const double rad, const double min_rad = -M_PI, const double max_rad = M_PI) { const auto value = std::fmod(rad, 2 * M_PI); if (min_rad < value && value <= max_rad) { return value; - } else { - return value - std::copysign(2 * M_PI, value); } + return value - std::copysign(2 * M_PI, value); } -bool isInLane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) +bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) { // check if goal is on a lane at appropriate angle const auto distance = boost::geometry::distance( @@ -72,7 +71,7 @@ bool isInLane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d return distance < th_distance; } -bool isInParkingSpace( +bool is_in_parking_space( const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) { for (const auto & parking_space : parking_spaces) { @@ -92,7 +91,7 @@ bool isInParkingSpace( return false; } -bool isInParkingLot( +bool is_in_parking_lot( const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) { for (const auto & parking_lot : parking_lots) { @@ -106,7 +105,7 @@ bool isInParkingLot( return false; } -double projectGoalToMap( +double project_goal_to_map( const lanelet::Lanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) { const lanelet::ConstLineString3d center_line = @@ -125,10 +124,10 @@ MissionPlannerLanelet2::MissionPlannerLanelet2(const rclcpp::NodeOptions & node_ using std::placeholders::_1; map_subscriber_ = create_subscription( "input/vector_map", rclcpp::QoS{10}.transient_local(), - std::bind(&MissionPlannerLanelet2::mapCallback, this, _1)); + std::bind(&MissionPlannerLanelet2::map_callback, this, _1)); } -void MissionPlannerLanelet2::mapCallback( +void MissionPlannerLanelet2::map_callback( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { route_handler_.setMap(*msg); @@ -141,9 +140,9 @@ void MissionPlannerLanelet2::mapCallback( is_graph_ready_ = true; } -bool MissionPlannerLanelet2::isRoutingGraphReady() const { return is_graph_ready_; } +bool MissionPlannerLanelet2::is_routing_graph_ready() const { return is_graph_ready_; } -void MissionPlannerLanelet2::visualizeRoute( +void MissionPlannerLanelet2::visualize_route( const autoware_auto_planning_msgs::msg::HADMapRoute & route) const { lanelet::ConstLanelets route_lanelets; @@ -163,33 +162,37 @@ void MissionPlannerLanelet2::visualizeRoute( } } - std_msgs::msg::ColorRGBA cl_route, cl_ll_borders, cl_end, cl_normal, cl_goal; - setColor(&cl_route, 0.2, 0.4, 0.2, 0.05); - setColor(&cl_goal, 0.2, 0.4, 0.4, 0.05); - setColor(&cl_end, 0.2, 0.2, 0.4, 0.05); - setColor(&cl_normal, 0.2, 0.4, 0.2, 0.05); - setColor(&cl_ll_borders, 1.0, 1.0, 1.0, 0.999); + std_msgs::msg::ColorRGBA cl_route; + std_msgs::msg::ColorRGBA cl_ll_borders; + std_msgs::msg::ColorRGBA cl_end; + std_msgs::msg::ColorRGBA cl_normal; + std_msgs::msg::ColorRGBA cl_goal; + set_color(&cl_route, 0.2, 0.4, 0.2, 0.05); + set_color(&cl_goal, 0.2, 0.4, 0.4, 0.05); + set_color(&cl_end, 0.2, 0.2, 0.4, 0.05); + set_color(&cl_normal, 0.2, 0.4, 0.2, 0.05); + set_color(&cl_ll_borders, 1.0, 1.0, 1.0, 0.999); visualization_msgs::msg::MarkerArray route_marker_array; - insertMarkerArray( + insert_marker_array( &route_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray(route_lanelets, cl_ll_borders, false)); - insertMarkerArray( + insert_marker_array( &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "route_lanelets", route_lanelets, cl_route)); - insertMarkerArray( + insert_marker_array( &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray("end_lanelets", end_lanelets, cl_end)); - insertMarkerArray( + insert_marker_array( &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "normal_lanelets", normal_lanelets, cl_normal)); - insertMarkerArray( + insert_marker_array( &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray("goal_lanelets", goal_lanelets, cl_goal)); marker_publisher_->publish(route_marker_array); } -bool MissionPlannerLanelet2::isGoalValid() const +bool MissionPlannerLanelet2::is_goal_valid() const { lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( @@ -198,11 +201,11 @@ bool MissionPlannerLanelet2::isGoalValid() const } const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position); - if (isInLane(closest_lanelet, goal_lanelet_pt)) { + if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose_.pose.position); const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); - const auto angle_diff = normalizeRadian(lane_yaw - goal_yaw); + const auto angle_diff = normalize_radian(lane_yaw - goal_yaw); constexpr double th_angle = M_PI / 4; @@ -213,13 +216,13 @@ bool MissionPlannerLanelet2::isGoalValid() const // check if goal is in parking space const auto parking_spaces = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); - if (isInParkingSpace(parking_spaces, goal_lanelet_pt)) { + if (is_in_parking_space(parking_spaces, goal_lanelet_pt)) { return true; } // check if goal is in parking lot const auto parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_); - if (isInParkingLot(parking_lots, goal_lanelet_pt)) { + if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) { return true; } @@ -230,11 +233,11 @@ bool MissionPlannerLanelet2::isGoalValid() const return false; } // check if goal pose is in shoulder lane - if (isInLane(closest_shoulder_lanelet, goal_lanelet_pt)) { + if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose_.pose.position); const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); - const auto angle_diff = normalizeRadian(lane_yaw - goal_yaw); + const auto angle_diff = normalize_radian(lane_yaw - goal_yaw); constexpr double th_angle = M_PI / 4; if (std::abs(angle_diff) < th_angle) { @@ -245,7 +248,7 @@ bool MissionPlannerLanelet2::isGoalValid() const return false; } -autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute() +autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::plan_route() { std::stringstream log_ss; for (const auto & checkpoint : checkpoints_) { @@ -259,7 +262,7 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( autoware_auto_planning_msgs::msg::HADMapRoute route_msg; RouteSections route_sections; - if (!isGoalValid()) { + if (!is_goal_valid()) { RCLCPP_WARN(get_logger(), "Goal is not valid! Please check position and angle of goal_pose"); return route_msg; } @@ -275,7 +278,7 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( // create local route sections route_handler_.setRouteLanelets(path_lanelets); const auto local_route_sections = route_handler_.createMapSegments(path_lanelets); - route_sections = combineConsecutiveRouteSections(route_sections, local_route_sections); + route_sections = combine_consecutive_route_sections(route_sections, local_route_sections); } if (route_handler_.isRouteLooped(route_sections)) { @@ -283,7 +286,7 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( return route_msg; } - refineGoalHeight(route_sections); + refine_goal_height(route_sections); route_msg.header.stamp = this->now(); route_msg.header.frame_id = map_frame_; @@ -294,12 +297,12 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( return route_msg; } -void MissionPlannerLanelet2::refineGoalHeight(const RouteSections & route_sections) +void MissionPlannerLanelet2::refine_goal_height(const RouteSections & route_sections) { const auto goal_lane_id = route_sections.back().preferred_primitive_id; lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id); const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position); - double goal_height = projectGoalToMap(goal_lanelet, goal_lanelet_pt); + double goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt); goal_pose_.pose.position.z = goal_height; checkpoints_.back().pose.position.z = goal_height; } diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp index 8e2e642f29512..1fdebd7cf76db 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp @@ -56,14 +56,14 @@ class MissionPlannerLanelet2 : public MissionPlanner rclcpp::Subscription::SharedPtr map_subscriber_; - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); - bool isGoalValid() const; - void refineGoalHeight(const RouteSections & route_sections); + void map_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + bool is_goal_valid() const; + void refine_goal_height(const RouteSections & route_sections); // virtual functions - bool isRoutingGraphReady() const; - autoware_auto_planning_msgs::msg::HADMapRoute planRoute(); - void visualizeRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const; + bool is_routing_graph_ready() const override; + autoware_auto_planning_msgs::msg::HADMapRoute plan_route() override; + void visualize_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const override; }; } // namespace mission_planner diff --git a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp index 17a2f3d582150..e0ffe51d188fd 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp @@ -28,14 +28,14 @@ bool exists(const std::unordered_set & set, const lanelet::Id & id) return set.find(id) != set.end(); } -std::string toString(const geometry_msgs::msg::Pose & pose) +std::string to_string(const geometry_msgs::msg::Pose & pose) { std::stringstream ss; ss << "(" << pose.position.x << ", " << pose.position.y << "," << pose.position.z << ")"; return ss.str(); } -void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) { cl->r = r; cl->g = g; @@ -43,13 +43,13 @@ void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doubl cl->a = a; } -void insertMarkerArray( +void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) { a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -std::vector> excludeSubtypeLaneletsWithDistance( +std::vector> exclude_subtype_lanelets_with_distance( const std::vector> & lls, const char subtype[]) { std::vector> exclude_subtype_lanelets; diff --git a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp index ca83eca5b1874..fd26c06f38b79 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp @@ -39,8 +39,8 @@ bool exists(const std::vector & vectors, const T & item) return false; } -void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a); -void insertMarkerArray( +void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a); +void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); -std::string toString(const geometry_msgs::msg::Pose & pose); +std::string to_string(const geometry_msgs::msg::Pose & pose); #endif // MISSION_PLANNER_LANELET2__UTILITY_FUNCTIONS_HPP_ diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/motion_velocity_smoother/CMakeLists.txt index 2995442ee1b67..64778b4b7ffbe 100644 --- a/planning/motion_velocity_smoother/CMakeLists.txt +++ b/planning/motion_velocity_smoother/CMakeLists.txt @@ -25,7 +25,6 @@ set(SMOOTHER_SRC src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp src/trajectory_utils.cpp - src/linear_interpolation.cpp src/resample.cpp ) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp deleted file mode 100644 index 10c0bca1ddddb..0000000000000 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/linear_interpolation.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ -#define MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ - -#include - -#include -#include -#include - -namespace motion_velocity_smoother -{ -namespace linear_interpolation -{ -boost::optional> interpolate( - const std::vector & base_index, const std::vector & base_value, - const std::vector & return_index); -} -} // namespace motion_velocity_smoother - -#endif // MOTION_VELOCITY_SMOOTHER__LINEAR_INTERPOLATION_HPP_ diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 6ea62a101e39b..dcc5079c9f9fb 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -125,7 +125,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node double extract_ahead_dist; // forward waypoints distance from current position [m] double extract_behind_dist; // backward waypoints distance from current position [m] double stop_dist_to_prohibit_engage; // prevent to move toward close stop point - double delta_yaw_threshold; // for closest index calculation + double ego_nearest_dist_threshold; // for ego's closest index calculation + double ego_nearest_yaw_threshold; // for ego's closest index calculation + resampling::ResampleParam post_resample_param; AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2 } node_param_{}; @@ -225,9 +227,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_closest_merged_velocity_; // helper functions - boost::optional findNearestIndex( - const TrajectoryPoints & points, const geometry_msgs::msg::Pose & p) const; - boost::optional findNearestIndexFromEgo(const TrajectoryPoints & points) const; + size_t findNearestIndexFromEgo(const TrajectoryPoints & points) const; bool isReverse(const TrajectoryPoints & points) const; void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp index 76fffd5f95901..97e4c974f9ad7 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -45,13 +45,13 @@ struct ResampleParam boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v_current, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold, - const ResampleParam & param, const bool use_zoh_for_v = true); + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v = true); boost::optional resampleTrajectory( const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds, - const bool use_zoh_for_v = true); + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 32e97b5511827..0603c8305cee9 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -74,7 +74,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, - [[maybe_unused]] const double delta_yaw_threshold) const override; + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const override; boost::optional applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 2d595e9c1a1d6..554bdcca890ef 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -16,7 +16,6 @@ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT #include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/linear_interpolation.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 0cbf3a2cbd733..d4b65e695bb89 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -48,7 +48,8 @@ class JerkFilteredSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const override; + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const override; void setParam(const Param & param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index c822c2625c29a..de1d3cd4491c3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -46,7 +46,7 @@ class L2PseudoJerkSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const override; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; void setParam(const Param & smoother_param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 145336211228f..dadb314edbb58 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -46,7 +46,7 @@ class LinfPseudoJerkSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const override; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; void setParam(const Param & smoother_param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 0d401af8e5ebf..0ec6330394a56 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -58,7 +58,7 @@ class SmootherBase virtual boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const = 0; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0; virtual boost::optional applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index ba3bb4805ae89..a1fee003703e3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -38,14 +38,12 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Pose; TrajectoryPoint calcInterpolatedTrajectoryPoint( - const TrajectoryPoints & trajectory, const Pose & target_pose); + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx); TrajectoryPoints extractPathAroundIndex( const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, const double & behind_length); -double calcArcLength(const TrajectoryPoints & trajectory, const int idx1, const int idx2); - std::vector calcArclengthArray(const TrajectoryPoints & trajectory); std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & trajectory); 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 d21c050caf9c5..8a1042fafa1e3 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml @@ -1,6 +1,7 @@ + @@ -16,6 +17,7 @@ + diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index f1a9c33781626..620db1717a454 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -4,11 +4,15 @@ motion_velocity_smoother 0.1.0 The motion_velocity_smoother package - Takamasa Horibe + Fumiya Watanabe - Yutaka Shimizu Apache License 2.0 + Takamasa Horibe + Fumiya Watanabe + Yutaka Shimizu + Makoto Kurihara + ament_cmake_auto eigen3_cmake_module diff --git a/planning/motion_velocity_smoother/src/linear_interpolation.cpp b/planning/motion_velocity_smoother/src/linear_interpolation.cpp deleted file mode 100644 index d77134302a486..0000000000000 --- a/planning/motion_velocity_smoother/src/linear_interpolation.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "motion_velocity_smoother/linear_interpolation.hpp" - -#include - -namespace motion_velocity_smoother -{ -/* - * linear interpolation - */ -namespace linear_interpolation -{ -boost::optional> interpolate( - const std::vector & sample_x, const std::vector & sample_value, - const std::vector & query_x) -{ - std::vector query_value; - auto isIncrease = [](const std::vector & x) { - if (x.empty()) { - return false; - } - for (size_t i = 0; i < x.size() - 1; ++i) { - if (x.at(i) > x.at(i + 1)) { - return false; - } - } - return true; - }; - - if (sample_x.empty() || sample_value.empty() || query_x.empty()) { - printf( - "[interpolate] some vector size is zero: sample_x.size() = %lu, sample_value.size() = %lu, " - "query_x.size() = %lu\n", - sample_x.size(), sample_value.size(), query_x.size()); - return {}; - } - - // check if inputs are valid - if ( - !isIncrease(sample_x) || !isIncrease(query_x) || query_x.front() < sample_x.front() || - sample_x.back() < query_x.back() || sample_x.size() != sample_value.size()) { - std::cerr << "[isIncrease] bad index, return false" << std::endl; - const bool b1 = !isIncrease(sample_x); - const bool b2 = !isIncrease(query_x); - const bool b3 = query_x.front() < sample_x.front(); - const bool b4 = sample_x.back() < query_x.back(); - const bool b5 = sample_x.size() != sample_value.size(); - printf("%d, %d, %d, %d, %d\n", b1, b2, b3, b4, b5); - printf("sample_x = [%f, %f]\n", sample_x.front(), sample_x.back()); - printf("query_x = [%f, %f]\n", query_x.front(), query_x.back()); - printf( - "sample_x.size() = %lu, sample_value.size() = %lu\n", sample_x.size(), sample_value.size()); - return {}; - } - - // calculate linear interpolation - int i = 0; - for (const auto idx : query_x) { - if (sample_x.at(i) == idx) { - query_value.push_back(sample_value.at(i)); - continue; - } - while (sample_x.at(i) < idx) { - ++i; - } - if (i <= 0 || static_cast(sample_x.size()) - 1 < i) { - std::cerr << "? something wrong. skip this idx." << std::endl; - continue; - } - - const double dist_base_idx = sample_x.at(i) - sample_x.at(i - 1); - const double dist_to_forward = sample_x.at(i) - idx; - const double dist_to_backward = idx - sample_x.at(i - 1); - if (dist_to_forward < 0.0 || dist_to_backward < 0.0) { - std::cerr << "?? something wrong. skip this idx. sample_x.at(i - 1) = " << sample_x.at(i - 1) - << ", idx = " << idx << ", sample_x.at(i) = " << sample_x.at(i) << std::endl; - continue; - } - - const double value = - (dist_to_backward * sample_value.at(i) + dist_to_forward * sample_value.at(i - 1)) / - dist_base_idx; - query_value.push_back(value); - } - return query_value; -} -} // namespace linear_interpolation -} // namespace motion_velocity_smoother 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 4f9aedfed0d49..ba0018e26ce77 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -148,7 +148,8 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter update_param("extract_ahead_dist", p.extract_ahead_dist); update_param("extract_behind_dist", p.extract_behind_dist); update_param("stop_dist_to_prohibit_engage", p.stop_dist_to_prohibit_engage); - update_param("delta_yaw_threshold", p.delta_yaw_threshold); + update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold); + update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold); } { @@ -245,7 +246,8 @@ void MotionVelocitySmootherNode::initCommonParam() p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0); p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0); p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage", 1.5); - p.delta_yaw_threshold = declare_parameter("delta_yaw_threshold", M_PI / 3.0); + p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); p.post_resample_param.max_trajectory_length = declare_parameter("post_max_trajectory_length", 300.0); p.post_resample_param.min_trajectory_length = @@ -379,7 +381,8 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // Note that output velocity is resampled by linear interpolation auto output_resampled = resampling::resampleTrajectory( output, current_odometry_ptr_->twist.twist.linear.x, current_pose_ptr_->pose, - node_param_.delta_yaw_threshold, node_param_.post_resample_param, false); + node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold, + node_param_.post_resample_param, false); if (!output_resampled) { RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory"); return; @@ -432,16 +435,10 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length - const auto input_closest = findNearestIndexFromEgo(traj_input); - - if (!input_closest) { - RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, 5000, "Cannot find the closest point from input trajectory"); - return prev_output_; - } + const size_t input_closest = findNearestIndexFromEgo(traj_input); auto traj_extracted = trajectory_utils::extractPathAroundIndex( - traj_input, *input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); + traj_input, input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); if (traj_extracted.empty()) { RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory"); return prev_output_; @@ -458,12 +455,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( applyExternalVelocityLimit(traj_extracted); // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close - const auto traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); - - if (!traj_extracted_closest) { - RCLCPP_WARN(get_logger(), "Cannot find the closest point from extracted trajectory"); - return prev_output_; - } + const size_t traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); // Apply velocity to approach stop point applyStopApproachingVelocity(traj_extracted); @@ -476,7 +468,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } // Smoothing velocity - if (!smoothVelocity(traj_extracted, *traj_extracted_closest, output)) { + if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) { return prev_output_; } @@ -500,17 +492,13 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x, - current_pose_ptr_->pose, node_param_.delta_yaw_threshold); + current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); if (!traj_resampled) { RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); return false; } - const auto traj_resampled_closest = findNearestIndexFromEgo(*traj_resampled); - - if (!traj_resampled_closest) { - RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); - return false; - } + const size_t traj_resampled_closest = findNearestIndexFromEgo(*traj_resampled); // Set 0[m/s] in the terminal point if (!traj_resampled->empty()) { @@ -523,7 +511,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Clip trajectory from closest point TrajectoryPoints clipped; clipped.insert( - clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end()); + clipped.end(), traj_resampled->begin() + traj_resampled_closest, traj_resampled->end()); std::vector debug_trajectories; if (!smoother_->apply( @@ -536,7 +524,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( traj_smoothed.insert( traj_smoothed.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); + traj_resampled->begin() + traj_resampled_closest); // For the endpoint of the trajectory if (!traj_smoothed.empty()) { @@ -545,10 +533,10 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Max velocity filter for safety trajectory_utils::applyMaximumVelocityLimit( - *traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); + traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); // Insert behind velocity for output's consistency - insertBehindVelocity(*traj_resampled_closest, type, traj_smoothed); + insertBehindVelocity(traj_resampled_closest, type, traj_smoothed); RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size()); if (publish_debug_trajs_) { @@ -567,10 +555,10 @@ bool MotionVelocitySmootherNode::smoothVelocity( for (auto & debug_trajectory : debug_trajectories) { debug_trajectory.insert( debug_trajectory.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); - for (size_t i = 0; i < *traj_resampled_closest; ++i) { + traj_resampled->begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { debug_trajectory.at(i).longitudinal_velocity_mps = - debug_trajectory.at(*traj_resampled_closest).longitudinal_velocity_mps; + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; } } } @@ -592,8 +580,27 @@ void MotionVelocitySmootherNode::insertBehindVelocity( output.at(i).longitudinal_velocity_mps = output.at(output_closest).longitudinal_velocity_mps; output.at(i).acceleration_mps2 = output.at(output_closest).acceleration_mps2; } else { + // TODO(planning/control team) deal with overlapped lanes with the same direction + const size_t seg_idx = [&]() { + // with distance and yaw thresholds + const auto opt_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + if (opt_nearest_seg_idx) { + return opt_nearest_seg_idx.get(); + } + + // with distance threshold + const auto opt_second_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold); + if (opt_second_nearest_seg_idx) { + return opt_second_nearest_seg_idx.get(); + } + + return motion_utils::findNearestSegmentIndex(prev_output_, output.at(i).pose.position); + }(); const auto prev_output_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose); + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose, seg_idx); // output should be always positive: TODO(Horibe) think better way output.at(i).longitudinal_velocity_mps = @@ -605,25 +612,20 @@ void MotionVelocitySmootherNode::insertBehindVelocity( void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const { - const auto closest_optional = findNearestIndexFromEgo(trajectory); - if (!closest_optional) { - return; - } - const auto closest = *closest_optional; + const size_t closest = findNearestIndexFromEgo(trajectory); // stop distance calculation const double stop_dist_lim{50.0}; double stop_dist{stop_dist_lim}; const auto stop_idx{motion_utils::searchZeroVelocityIndex(trajectory)}; if (stop_idx) { - stop_dist = trajectory_utils::calcArcLength(trajectory, closest, *stop_idx); - stop_dist = closest > *stop_idx ? stop_dist : -stop_dist; + stop_dist = motion_utils::calcSignedArcLength(trajectory, closest, *stop_idx); } else { stop_dist = closest > 0 ? stop_dist : -stop_dist; } Float32Stamped dist_to_stopline{}; dist_to_stopline.stamp = this->now(); - dist_to_stopline.data = std::max(-stop_dist_lim, std::min(stop_dist_lim, stop_dist)); + dist_to_stopline.data = std::clamp(stop_dist, -stop_dist_lim, stop_dist_lim); pub_dist_to_stopline_->publish(dist_to_stopline); } @@ -646,8 +648,11 @@ MotionVelocitySmootherNode::calcInitialMotion( return std::make_pair(initial_motion, type); } - const auto prev_output_closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_traj, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_traj, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto prev_output_closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + prev_traj, current_pose_ptr_->pose, current_seg_idx); // when velocity tracking deviation is large const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps}; @@ -716,7 +721,10 @@ void MotionVelocitySmootherNode::overwriteStopPoint( } // Get Closest Point from Output - const auto nearest_output_point_idx = findNearestIndex(output, input.at(*stop_idx).pose); + // TODO(planning/control team) deal with overlapped lanes with the same directions + const auto nearest_output_point_idx = motion_utils::findNearestIndex( + output, input.at(*stop_idx).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); // check over velocity bool is_stop_velocity_exceeded{false}; @@ -760,13 +768,10 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( 0, traj.size(), max_velocity_with_deceleration_, traj); - const auto closest_idx = findNearestIndexFromEgo(traj); - if (!closest_idx) { - return; - } + const size_t closest_idx = findNearestIndexFromEgo(traj); double dist = 0.0; - for (size_t idx = *closest_idx; idx < traj.size() - 1; ++idx) { + for (size_t idx = closest_idx; idx < traj.size() - 1; ++idx) { dist += tier4_autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1)); if (dist > external_velocity_limit_dist_) { trajectory_utils::applyMaximumVelocityLimit( @@ -823,8 +828,11 @@ void MotionVelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose); + trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose, current_seg_idx); Float32Stamped vel_data{}; vel_data.stamp = this->now(); @@ -834,8 +842,11 @@ void MotionVelocitySmootherNode::publishClosestVelocity( void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + trajectory, current_pose_ptr_->pose, current_seg_idx); auto publishFloat = [=](const double data, const auto pub) { Float32Stamped msg{}; @@ -872,8 +883,11 @@ void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final { prev_output_ = final_result; - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(final_result, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + final_result, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + final_result, current_pose_ptr_->pose, current_seg_idx); prev_closest_point_ = closest_point; } @@ -899,8 +913,11 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit double MotionVelocitySmootherNode::calcTravelDistance() const { - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_output_, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + prev_output_, current_pose_ptr_->pose, current_seg_idx); if (prev_closest_point_) { const double travel_dist = @@ -926,17 +943,11 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( return trajectory; } -boost::optional MotionVelocitySmootherNode::findNearestIndexFromEgo( - const TrajectoryPoints & points) const -{ - return findNearestIndex(points, current_pose_ptr_->pose); -} - -boost::optional MotionVelocitySmootherNode::findNearestIndex( - const TrajectoryPoints & points, const geometry_msgs::msg::Pose & p) const +size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { - return motion_utils::findNearestIndex( - points, p, std::numeric_limits::max(), node_param_.delta_yaw_threshold); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); } bool MotionVelocitySmootherNode::isReverse(const TrajectoryPoints & points) const diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index f04a564bbdeda..674234ab287e4 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -26,16 +26,15 @@ namespace resampling { boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v_current, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold, - const ResampleParam & param, const bool use_zoh_for_v) + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v) { // Arc length from the initial point to the closest point - const auto negative_front_arclength_value = motion_utils::calcSignedArcLength( - input, current_pose, 0, std::numeric_limits::max(), delta_yaw_threshold); - if (!negative_front_arclength_value) { - return {}; - } - const auto front_arclength_value = std::fabs(*negative_front_arclength_value); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, 0); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint(input, current_pose); @@ -145,8 +144,8 @@ boost::optional resampleTrajectory( boost::optional resampleTrajectory( const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds, - const bool use_zoh_for_v) + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) { // input arclength const double trajectory_length = motion_utils::calcArcLength(input); @@ -169,12 +168,12 @@ boost::optional resampleTrajectory( // Step1. Resample front trajectory // Arc length from the initial point to the closest point - const auto negative_front_arclength_value = motion_utils::calcSignedArcLength( - input, current_pose, 0, std::numeric_limits::max(), delta_yaw_threshold); - if (!negative_front_arclength_value) { - return {}; - } - const auto front_arclength_value = std::fabs(*negative_front_arclength_value); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, + static_cast(0)); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); for (double s = 0.0; s <= front_arclength_value; s += nominal_ds) { out_arclength.push_back(s); } diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index e32d37b403a15..a43f67b00b53d 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -234,7 +234,8 @@ bool AnalyticalJerkConstrainedSmoother::apply( boost::optional AnalyticalJerkConstrainedSmoother::resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, - [[maybe_unused]] const double delta_yaw_threshold) const + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const { TrajectoryPoints output; if (input.empty()) { 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 9a55dbe673f63..b65147ccfabb9 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 @@ -14,6 +14,8 @@ #include "motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp" +#include "interpolation/linear_interpolation.hpp" + #include #include @@ -232,28 +234,36 @@ bool calcStopVelocityWithConstantJerkAccLimit( dists.push_back(dist); } - const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, dists); - const auto acc_at_wp = linear_interpolation::interpolate(xs, as, dists); - const auto jerk_at_wp = linear_interpolation::interpolate(xs, js, dists); - if (!vel_at_wp || !acc_at_wp || !jerk_at_wp) { - RCLCPP_DEBUG(rclcpp::get_logger("velocity_planning_utils"), "Interpolation error"); + if ( + !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(dists) || + !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(dists)) { 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()) { + 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); + // 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) - << ", j: " << jerk_at_wp->at(i) << std::endl; + ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) + << ", j: " << jerk_at_wp.at(i) << std::endl; } RCLCPP_DEBUG( rclcpp::get_logger("velocity_planning_utils"), "Interpolated = %s", ssi.str().c_str()); - for (size_t i = 0; i < vel_at_wp->size(); ++i) { - output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i); - output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i); + for (size_t i = 0; i < vel_at_wp.size(); ++i) { + output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); + output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp.at(i); } - for (size_t i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) { + for (size_t i = start_index + vel_at_wp.size(); i < output_trajectory.size(); ++i) { output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; output_trajectory.at(i).acceleration_mps2 = 0.0; } diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 2fcfc1c77fd8a..d3b8d79e2f376 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -100,10 +100,11 @@ bool JerkFilteredSmoother::apply( debug_trajectories[2] = filtered; // Resample TrajectoryPoints for Optimization + // TODO(planning/control team) deal with overlapped lanes with the same direction const auto initial_traj_pose = filtered.front().pose; auto opt_resampled_trajectory = resampling::resampleTrajectory( filtered, v0, initial_traj_pose, std::numeric_limits::max(), - base_param_.resample_param); + std::numeric_limits::max(), base_param_.resample_param); if (!opt_resampled_trajectory) { RCLCPP_WARN(logger_, "Resample failed!"); @@ -471,10 +472,11 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( boost::optional JerkFilteredSmoother::resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, current_pose, delta_yaw_threshold, base_param_.resample_param, + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, smoother_param_.jerk_filter_ds); } diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index da3764e307cc7..6b2ae568f0052 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -223,10 +223,11 @@ bool L2PseudoJerkSmoother::apply( boost::optional L2PseudoJerkSmoother::resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const + const double nearest_dist_threshold, const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param); + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); } } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 8030a0ff5c1da..90e72f8b083d8 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -235,10 +235,11 @@ bool LinfPseudoJerkSmoother::apply( boost::optional LinfPseudoJerkSmoother::resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const + const double nearest_dist_threshold, const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param); + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); } } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 8cb38496a8e67..629ba1b15040a 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -16,7 +16,6 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" -#include "motion_velocity_smoother/linear_interpolation.hpp" #include #include @@ -59,7 +58,7 @@ inline double integ_v(double v0, double a0, double j0, double t) inline double integ_a(double a0, double j0, double t) { return a0 + j0 * t; } TrajectoryPoint calcInterpolatedTrajectoryPoint( - const TrajectoryPoints & trajectory, const Pose & target_pose) + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx) { TrajectoryPoint traj_p{}; traj_p.pose = target_pose; @@ -76,17 +75,14 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( return traj_p; } - const size_t segment_idx = - motion_utils::findNearestSegmentIndex(trajectory, target_pose.position); - - auto v1 = getTransVector3(trajectory.at(segment_idx).pose, trajectory.at(segment_idx + 1).pose); - auto v2 = getTransVector3(trajectory.at(segment_idx).pose, target_pose); + auto v1 = getTransVector3(trajectory.at(seg_idx).pose, trajectory.at(seg_idx + 1).pose); + auto v2 = getTransVector3(trajectory.at(seg_idx).pose, target_pose); // calc internal proportion const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; { - const auto & seg_pt = trajectory.at(segment_idx); - const auto & next_pt = trajectory.at(segment_idx + 1); + const auto & seg_pt = trajectory.at(seg_idx); + const auto & next_pt = trajectory.at(seg_idx + 1); traj_p.pose = tier4_autoware_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); traj_p.longitudinal_velocity_mps = interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); @@ -140,41 +136,20 @@ TrajectoryPoints extractPathAroundIndex( return extracted_traj; } -double calcArcLength(const TrajectoryPoints & path, const int idx1, const int idx2) +std::vector calcArclengthArray(const TrajectoryPoints & trajectory) { - if (idx1 == idx2) { // zero distance - return 0.0; - } - - if ( - idx1 < 0 || idx2 < 0 || static_cast(path.size()) - 1 < idx1 || - static_cast(path.size()) - 1 < idx2) { - RCLCPP_ERROR( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), - "invalid index"); - return 0.0; - } - - const int idx_from = std::min(idx1, idx2); - const int idx_to = std::max(idx1, idx2); - double dist_sum = 0.0; - for (int i = idx_from; i < idx_to; ++i) { - dist_sum += tier4_autoware_utils::calcDistance2d(path.at(i), path.at(i + 1)); + if (trajectory.empty()) { + return {}; } - return dist_sum; -} -std::vector calcArclengthArray(const TrajectoryPoints & trajectory) -{ - std::vector arclength; + std::vector arclength(trajectory.size()); double dist = 0.0; - arclength.clear(); - arclength.push_back(dist); + arclength.front() = dist; for (unsigned int i = 1; i < trajectory.size(); ++i) { const TrajectoryPoint tp = trajectory.at(i); const TrajectoryPoint tp_prev = trajectory.at(i - 1); dist += tier4_autoware_utils::calcDistance2d(tp.pose, tp_prev.pose); - arclength.push_back(dist); + arclength.at(i) = dist; } return arclength; } @@ -211,23 +186,32 @@ std::vector calcTrajectoryCurvatureFrom3Points( } // calculate curvature by circle fitting from three points - std::vector k_arr; - for (size_t i = idx_dist; i < trajectory.size() - idx_dist; ++i) { + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); try { - const auto p0 = getPoint(trajectory.at(i - idx_dist)); - const auto p1 = getPoint(trajectory.at(i)); - const auto p2 = getPoint(trajectory.at(i + idx_dist)); - k_arr.push_back(calcCurvature(p0, p1, p2)); - } catch (...) { - k_arr.push_back(0.0); // points are too close. No curvature. + curvature = calcCurvature(p0, p1, p2); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s", + e.what()); + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature + } else { + curvature = 0.0; + } } + k_arr.at(i) = curvature; } + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); - // first and last curvature is copied from next value - for (size_t i = 0; i < idx_dist; ++i) { - k_arr.insert(k_arr.begin(), k_arr.front()); - k_arr.push_back(k_arr.back()); - } return k_arr; } @@ -520,21 +504,26 @@ boost::optional applyDecelFilterWithJerkConstraint( distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); }); const std::vector distance{distance_all.begin() + start_index, it_end}; - const auto vel_at_wp = linear_interpolation::interpolate(xs, vs, distance); - const auto acc_at_wp = linear_interpolation::interpolate(xs, as, distance); + if ( + !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distance) || + !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distance)) { + return {}; + } - if (!vel_at_wp || !acc_at_wp) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), - "interpolation error"); + if ( + xs.size() < 2 || vs.size() < 2 || as.size() < 2 || distance.empty() || + distance.front() < xs.front() || xs.back() < distance.back()) { return {}; } - for (unsigned int i = 0; i < vel_at_wp->size(); ++i) { - output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp->at(i); - output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp->at(i); + const auto vel_at_wp = interpolation::lerp(xs, vs, distance); + const auto acc_at_wp = interpolation::lerp(xs, as, distance); + + for (unsigned int i = 0; i < vel_at_wp.size(); ++i) { + output_trajectory.at(start_index + i).longitudinal_velocity_mps = vel_at_wp.at(i); + output_trajectory.at(start_index + i).acceleration_mps2 = acc_at_wp.at(i); } - for (unsigned int i = start_index + vel_at_wp->size(); i < output_trajectory.size(); ++i) { + for (unsigned int i = start_index + vel_at_wp.size(); i < output_trajectory.size(); ++i) { output_trajectory.at(i).longitudinal_velocity_mps = decel_target_vel; output_trajectory.at(i).acceleration_mps2 = 0.0; } diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 5805ce4753e0a..667c0916e2aba 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -203,6 +203,9 @@ struct TrajectoryParam double acceleration_for_non_deceleration_range; int num_fix_points_for_extending; double max_dist_for_extending_end_point; + + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; }; struct MPTParam 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 fe9c91ff22c65..1c12ece517e89 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 @@ -294,10 +294,6 @@ class MPTOptimizer const std::vector & ref_points, std::shared_ptr debug_data_ptr) const; - size_t findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) const; - public: MPTOptimizer( const bool is_showing_debug_info, const TrajectoryParam & traj_param, diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp index a3b229ba55276..b21254ef20297 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp @@ -18,6 +18,7 @@ #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" @@ -115,6 +116,9 @@ std::vector interpolate2DTraj const std::vector & base_x, const std::vector & base_y, const std::vector & base_yaw, const double resolution); +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution); + template std::vector getInterpolatedPoints( const T & points, const double delta_arc_length, const double offset = 0) diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index e1ac7eba874ed..693b19afb0e4b 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -3,10 +3,12 @@ obstacle_avoidance_planner 0.1.0 - The path_planner package + The obstacle_avoidance_planner package Takayuki Murooka Apache License 2.0 + Takayuki Murooka + ament_cmake_auto autoware_cmake diff --git a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp index 86322802e941c..078a453cb461a 100644 --- a/planning/obstacle_avoidance_planner/src/costmap_generator.cpp +++ b/planning/obstacle_avoidance_planner/src/costmap_generator.cpp @@ -85,6 +85,7 @@ bool isAvoidingObject( return false; } + // TODO(murooka) remove findNearestIndex without any constraints const int nearest_idx = motion_utils::findNearestIndex( path_points, object.kinematics.initial_pose_with_covariance.pose.position); const auto nearest_path_point = path_points[nearest_idx]; @@ -272,6 +273,7 @@ cv::Mat CostmapGenerator::drawObstaclesOnImage( // fill between objects in the same side const auto get_closest_obj_point = [&](size_t idx) { + // TODO(murooka) remove findNearestIndex without any constraints const auto & path_point = path_points.at(motion_utils::findNearestIndex(path_points, obj_positions.at(idx))); double min_dist = std::numeric_limits::min(); diff --git a/planning/obstacle_avoidance_planner/src/cv_utils.cpp b/planning/obstacle_avoidance_planner/src/cv_utils.cpp index 3a0275f1550d8..ab77f05dd9f7b 100644 --- a/planning/obstacle_avoidance_planner/src/cv_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/cv_utils.cpp @@ -213,6 +213,7 @@ std::vector getCVPolygon( const std::vector & path_points, const cv::Mat & clearance_map, const nav_msgs::msg::MapMetaData & map_info) { + // TODO(murooka) remove findNearestIndex without any constraints const int nearest_idx = motion_utils::findNearestIndex( path_points, object.kinematics.initial_pose_with_covariance.pose.position); const auto nearest_path_point = path_points[nearest_idx]; @@ -249,6 +250,7 @@ std::vector getExtendedCVPolygon( } const Edges edges = optional_edges.get(); + // TODO(murooka) remove findNearestIndex without any constraints const int nearest_polygon_idx = motion_utils::findNearestIndex(points_in_image, edges.origin); std::vector cv_polygon; if (edges.back_idx == nearest_polygon_idx || edges.front_idx == nearest_polygon_idx) { diff --git a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp index 70a9c1cec08e0..c50f8d724aaf4 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp @@ -236,10 +236,9 @@ std::vector EBPathOptimizer::getFixedPoints( std::vector empty_points; return empty_points; } - const auto opt_begin_idx = motion_utils::findNearestIndex( - prev_trajs->smoothed_trajectory, ego_pose, std::numeric_limits::max(), - traj_param_.delta_yaw_threshold_for_closest_point); - const int begin_idx = opt_begin_idx ? *opt_begin_idx : 0; + const size_t begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + prev_trajs->smoothed_trajectory, ego_pose, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold); const int backward_fixing_idx = std::max( static_cast( begin_idx - diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index f06fbe2120b90..1dea0206252eb 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -178,7 +178,7 @@ Eigen::Vector2d getState( return kinematics; } -std::vector slerpYawFromReferencePoints(const std::vector & ref_points) +std::vector splineYawFromReferencePoints(const std::vector & ref_points) { if (ref_points.size() == 1) { return {ref_points.front().yaw}; @@ -188,7 +188,19 @@ std::vector slerpYawFromReferencePoints(const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold, const double yaw_threshold) +{ + const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + + const auto nearest_idx_optional = + motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); + return nearest_idx_optional ? *nearest_idx_optional + : motion_utils::findNearestIndex(points_with_yaw, pose.position); } } // namespace @@ -434,10 +446,10 @@ void MPTOptimizer::calcPlanningFromEgo(std::vector & ref_points) */ // assign fix kinematics - const size_t nearest_ref_idx = findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(ref_points), current_ego_pose_, - traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); + const size_t nearest_ref_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(ref_points)), + current_ego_pose_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold); // calculate cropped_ref_points.at(nearest_ref_idx) with yaw const geometry_msgs::msg::Pose nearest_ref_pose = [&]() -> geometry_msgs::msg::Pose { @@ -473,10 +485,11 @@ std::vector MPTOptimizer::getFixedReferencePoints( } const auto & prev_ref_points = prev_trajs->mpt_ref_points; - const int nearest_prev_ref_idx = static_cast(findNearestIndexWithSoftYawConstraints( - points_utils::convertToPoints(prev_ref_points), current_ego_pose_, - traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point)); + const int nearest_prev_ref_idx = + static_cast(motion_utils::findFirstNearestIndexWithSoftConstraints( + points_utils::convertToPosesWithYawEstimation(points_utils::convertToPoints(prev_ref_points)), + current_ego_pose_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold)); // calculate begin_prev_ref_idx const int begin_prev_ref_idx = [&]() { @@ -723,8 +736,14 @@ boost::optional MPTOptimizer::executeOptimization( const size_t D_x = vehicle_model_ptr_->getDimX(); if (prev_trajs && prev_trajs->mpt_ref_points.size() > 1) { - const size_t seg_idx = - motion_utils::findNearestSegmentIndex(prev_trajs->mpt_ref_points, ref_points.front().p); + geometry_msgs::msg::Pose ref_front_point; + ref_front_point.position = ref_points.front().p; + ref_front_point.orientation = + tier4_autoware_utils::createQuaternionFromYaw(ref_points.front().yaw); + + const size_t seg_idx = findNearestIndexWithSoftYawConstraints( + points_utils::convertToPoints(prev_trajs->mpt_ref_points), ref_front_point, + traj_param_.ego_nearest_dist_threshold, traj_param_.ego_nearest_yaw_threshold); double offset = motion_utils::calcLongitudinalOffsetToSegment( prev_trajs->mpt_ref_points, seg_idx, ref_points.front().p); @@ -1223,21 +1242,9 @@ std::vector MPTOptimizer::get return traj_points; } -size_t MPTOptimizer::findNearestIndexWithSoftYawConstraints( - const std::vector & points, const geometry_msgs::msg::Pose & pose, - const double dist_threshold, const double yaw_threshold) const -{ - const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); - - const auto nearest_idx_optional = - motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); - return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points_with_yaw, pose.position); -} - void MPTOptimizer::calcOrientation(std::vector & ref_points) const { - const auto yaw_angles = slerpYawFromReferencePoints(ref_points); + const auto yaw_angles = splineYawFromReferencePoints(ref_points); for (size_t i = 0; i < ref_points.size(); ++i) { if (ref_points.at(i).fix_kinematic_state) { continue; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 4fe6aca6d4992..0bd3bf97dd541 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -38,38 +38,27 @@ namespace { -template -size_t searchExtendedZeroVelocityIndex( - const std::vector & fine_points, const std::vector & vel_points) -{ - const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points); - const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; - return motion_utils::findNearestIndex(fine_points, vel_points.at(zero_vel_idx).pose.position); -} - bool isPathShapeChanged( const geometry_msgs::msg::Pose & ego_pose, const std::vector & path_points, const std::unique_ptr> & prev_path_points, - const double max_mpt_length, const double max_path_shape_change_dist, - const double delta_yaw_threshold) + const double max_mpt_length, const double max_path_shape_change_dist, const double dist_threshold, + const double yaw_threshold) { if (!prev_path_points) { return false; } // truncate prev points from ego pose to fixed end points - const auto opt_prev_begin_idx = motion_utils::findNearestIndex( - *prev_path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); - const size_t prev_begin_idx = opt_prev_begin_idx ? *opt_prev_begin_idx : 0; + const auto prev_begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + *prev_path_points, ego_pose, dist_threshold, yaw_threshold); const auto truncated_prev_points = points_utils::clipForwardPoints(*prev_path_points, prev_begin_idx, max_mpt_length); // truncate points from ego pose to fixed end points - const auto opt_begin_idx = motion_utils::findNearestIndex( - path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); - const size_t begin_idx = opt_begin_idx ? *opt_begin_idx : 0; + const auto begin_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_points, ego_pose, dist_threshold, yaw_threshold); const auto truncated_points = points_utils::clipForwardPoints(path_points, begin_idx, max_mpt_length); @@ -192,7 +181,7 @@ std::tuple, std::vector> calcVehicleCirclesInfo( for (const auto & traj_point : traj_points) { points.push_back(traj_point.pose.position); } - const auto yaw_vec = interpolation::slerpYawFromPoints(points); + const auto yaw_vec = interpolation::splineYawFromPoints(points); for (size_t i = 0; i < traj_points.size(); ++i) { traj_points.at(i).pose.orientation = @@ -200,16 +189,60 @@ std::tuple, std::vector> calcVehicleCirclesInfo( } } +template +[[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ + const auto nearest_idx_optional = + motion_utils::findNearestIndex(points, pose, dist_threshold, yaw_threshold); + return nearest_idx_optional ? *nearest_idx_optional + : motion_utils::findNearestIndex(points, pose.position); +} + +template <> [[maybe_unused]] size_t findNearestIndexWithSoftYawConstraints( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold) { const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + return findNearestIndexWithSoftYawConstraints( + points_with_yaw, pose, dist_threshold, yaw_threshold); +} + +template +[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ const auto nearest_idx_optional = - motion_utils::findNearestIndex(points_with_yaw, pose, dist_threshold, yaw_threshold); + motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); return nearest_idx_optional ? *nearest_idx_optional - : motion_utils::findNearestIndex(points_with_yaw, pose.position); + : motion_utils::findNearestSegmentIndex(points, pose.position); +} + +template <> +[[maybe_unused]] size_t findNearestSegmentIndexWithSoftYawConstraints( + const std::vector & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold, const double yaw_threshold) +{ + const auto points_with_yaw = points_utils::convertToPosesWithYawEstimation(points); + + return findNearestSegmentIndexWithSoftYawConstraints( + points_with_yaw, pose, dist_threshold, yaw_threshold); +} + +template +size_t searchExtendedZeroVelocityIndex( + const std::vector & fine_points, const std::vector & vel_points, + const double yaw_threshold) +{ + const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(vel_points); + const size_t zero_vel_idx = opt_zero_vel_idx ? opt_zero_vel_idx.get() : vel_points.size() - 1; + return findNearestIndexWithSoftYawConstraints( + fine_points, vel_points.at(zero_vel_idx).pose, std::numeric_limits::max(), + yaw_threshold); } } // namespace @@ -344,6 +377,11 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n declare_parameter("object.avoiding_object_type.pedestrian", true); traj_param_.is_avoiding_animal = declare_parameter("object.avoiding_object_type.animal", true); + + // ego nearest search + traj_param_.ego_nearest_dist_threshold = + declare_parameter("ego_nearest_dist_threshold"); + traj_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); } { // elastic band parameter @@ -986,8 +1024,8 @@ bool ObstacleAvoidancePlanner::checkReplan( traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points; if (isPathShapeChanged( current_ego_pose_, path_points, prev_path_points_ptr_, max_mpt_length, - max_path_shape_change_dist_for_replan_, - traj_param_.delta_yaw_threshold_for_closest_point)) { + max_path_shape_change_dist_for_replan_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold)) { RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path shape was changed."); resetPrevOptimization(); return true; @@ -1110,15 +1148,9 @@ void ObstacleAvoidancePlanner::calcVelocity( std::vector & traj_points) const { for (size_t i = 0; i < traj_points.size(); i++) { - const size_t nearest_seg_idx = [&]() { - const auto opt_seg_idx = motion_utils::findNearestSegmentIndex( - path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, - traj_param_.delta_yaw_threshold_for_closest_point); - if (opt_seg_idx) { - return opt_seg_idx.get(); - } - return motion_utils::findNearestSegmentIndex(path_points, traj_points.at(i).pose.position); - }(); + const size_t nearest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( + path_points, traj_points.at(i).pose, traj_param_.delta_dist_threshold_for_closest_point, + traj_param_.delta_yaw_threshold_for_closest_point); // add this line not to exceed max index size const size_t max_idx = std::min(nearest_seg_idx + 1, path_points.size() - 1); @@ -1144,8 +1176,9 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( const auto & map_info = cv_maps.map_info; const auto & road_clearance_map = cv_maps.clearance_map; - const size_t nearest_idx = - motion_utils::findNearestIndex(traj_points, current_ego_pose_.position); + const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_points, current_ego_pose_, traj_param_.ego_nearest_dist_threshold, + traj_param_.ego_nearest_yaw_threshold); // NOTE: Some end trajectory points will be ignored to check if outside the drivable area // since these points might be outside drivable area if only end reference points have high @@ -1469,8 +1502,9 @@ ObstacleAvoidancePlanner::alignVelocity( // search zero velocity index of fine_traj_points const size_t zero_vel_fine_traj_idx = [&]() { // zero velocity for being outside drivable area - const size_t zero_vel_traj_idx = - searchExtendedZeroVelocityIndex(fine_traj_points_with_path_zero_vel, traj_points); + const size_t zero_vel_traj_idx = searchExtendedZeroVelocityIndex( + fine_traj_points_with_path_zero_vel, traj_points, + traj_param_.delta_yaw_threshold_for_closest_point); // zero velocity in path points if (opt_zero_vel_path_idx) { @@ -1492,15 +1526,10 @@ ObstacleAvoidancePlanner::alignVelocity( } const auto & target_pose = fine_traj_points_with_vel[i].pose; - const auto closest_seg_idx_optional = motion_utils::findNearestSegmentIndex( + const size_t closest_seg_idx = findNearestSegmentIndexWithSoftYawConstraints( truncated_points, target_pose, traj_param_.delta_dist_threshold_for_closest_point, traj_param_.delta_yaw_threshold_for_closest_point); - const auto closest_seg_idx = - closest_seg_idx_optional - ? *closest_seg_idx_optional - : motion_utils::findNearestSegmentIndex(truncated_points, target_pose.position); - // lerp z fine_traj_points_with_vel[i].pose.position.z = lerpPoseZ(truncated_points, target_pose.position, closest_seg_idx); diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index 90e9daccb0a89..f5f030dcd4ac8 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -110,7 +110,7 @@ std::array, 2> validatePoints( } // only two points is supported -std::vector slerpTwoPoints( +std::vector splineTwoPoints( std::vector base_s, std::vector base_x, const double begin_diff, const double end_diff, std::vector new_s) { @@ -259,8 +259,8 @@ std::vector interpolate2DPoints( } // spline interpolation - const std::vector interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const std::vector interpolated_y = interpolation::slerp(base_s, base_y, new_s); + const std::vector interpolated_x = interpolation::spline(base_s, base_x, new_s); + const std::vector interpolated_y = interpolation::spline(base_s, base_y, new_s); for (size_t i = 0; i < interpolated_x.size(); ++i) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { return std::vector{}; @@ -296,9 +296,9 @@ std::vector interpolateConnec // spline interpolation const auto interpolated_x = - slerpTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); + splineTwoPoints(base_s, base_x, std::cos(begin_yaw), std::cos(end_yaw), new_s); const auto interpolated_y = - slerpTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); + splineTwoPoints(base_s, base_y, std::sin(begin_yaw), std::sin(end_yaw), new_s); for (size_t i = 0; i < interpolated_x.size(); i++) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { @@ -343,9 +343,51 @@ std::vector interpolate2DTraj const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); // spline interpolation - const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); + const auto interpolated_x = interpolation::spline(base_s, base_x, new_s); + const auto interpolated_y = interpolation::spline(base_s, base_y, new_s); + const auto interpolated_yaw = interpolation::spline(base_s, monotonic_base_yaw, new_s); + + for (size_t i = 0; i < interpolated_x.size(); i++) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } + } + + std::vector interpolated_points; + for (size_t i = 0; i < interpolated_x.size(); i++) { + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = interpolated_x[i]; + point.pose.position.y = interpolated_y[i]; + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); + interpolated_points.push_back(point); + } + + return interpolated_points; +} + +std::vector interpolate2DTrajectoryPoints( + const std::vector & base_x, const std::vector & base_y, const double resolution) +{ + if (base_x.empty() || base_y.empty()) { + return std::vector{}; + } + std::vector base_s = calcEuclidDist(base_x, base_y); + if (base_s.empty() || base_s.size() == 1) { + return std::vector{}; + } + std::vector new_s; + for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) { + new_s.push_back(i); + } + + // spline interpolation + // x = interpolated[0], y = interpolated[1], yaw = interpolated[2] + std::array, 3> interpolated = + interpolation::slerp2dFromXY(base_s, base_x, base_y, new_s); + const auto & interpolated_x = interpolated[0]; + const auto & interpolated_y = interpolated[1]; + const auto & interpolated_yaw = interpolated[2]; for (size_t i = 0; i < interpolated_x.size(); i++) { if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { @@ -372,7 +414,7 @@ std::vector getInterpolatedTr { std::array, 3> validated_pose = validateTrajectoryPoints(points); return interpolation_utils::interpolate2DTrajectoryPoints( - validated_pose.at(0), validated_pose.at(1), validated_pose.at(2), delta_arc_length); + validated_pose.at(0), validated_pose.at(1), delta_arc_length); } std::vector getConnectedInterpolatedPoints( 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 ae511e42bc2e9..606be776d98f7 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -90,16 +90,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, const Trajectory & traj, std::vector & target_obstacles); - std::vector calcNearestCollisionPoint( - const size_t & first_within_idx, - const std::vector & collision_points, - const Trajectory & decimated_traj, const bool is_driving_forward); double calcCollisionTimeMargin( const geometry_msgs::msg::Pose & current_pose, const double current_vel, - const geometry_msgs::msg::Point & nearest_collision_point, - const PredictedObject & predicted_object, const size_t first_within_idx, - const Trajectory & decimated_traj, - const std::vector & decimated_traj_polygons, + const std::vector & collision_points, + const PredictedObject & predicted_object, const Trajectory & decimated_traj, const bool is_driving_forward); void publishVelocityLimit(const boost::optional & vel_limit); void publishDebugData(const DebugData & debug_data) const; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index af9ecb0b12b05..e4b38b490d3f3 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace polygon_utils @@ -34,28 +35,40 @@ namespace bg = boost::geometry; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -boost::optional getFirstCollisionIndex( - const std::vector & traj_polygons, const Polygon2d & obj_polygon, - const std_msgs::msg::Header & obj_header, - std::vector & collision_points); +boost::optional getCollisionIndex( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & collision_points, + const double max_dist = std::numeric_limits::max()); -boost::optional getFirstNonCollisionIndex( - const std::vector & base_polygons, +std::vector getCollisionPoints( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const size_t start_idx); + const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const double vehicle_max_longitudinal_offset, const bool is_driving_forward, + const double max_dist = std::numeric_limits::max(), + const double max_prediction_time_for_collision_check = std::numeric_limits::max()); -boost::optional willCollideWithSurroundObstacle( +std::vector willCollideWithSurroundObstacle( const autoware_auto_planning_msgs::msg::Trajectory & traj, const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const double max_dist, - const double ego_obstacle_overlap_time_threshold, + const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const double max_dist, const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, - std::vector & collision_geom_points); + const double vehicle_max_longitudinal_offset, const bool is_driving_forward); std::vector createOneStepPolygons( const autoware_auto_planning_msgs::msg::Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + +geometry_msgs::msg::PointStamped calcNearestCollisionPoint( + const size_t & first_within_idx, + const std::vector & collision_points, + const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, + const double vehicle_max_longitudinal_offset, const bool is_driving_forward); } // namespace polygon_utils #endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index dea636631afa4..399fe1a39608b 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -2,12 +2,16 @@ obstacle_cruise_planner 0.1.0 - The Stop/Slow down planner for dynamic obstacles + The obstacle_cruise_planner package Takayuki Murooka + Yutaka Shimizu Apache License 2.0 + Takayuki Murooka + Yutaka Shimizu + ament_cmake_auto autoware_cmake diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 15b468299c7fb..d01602f176c35 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -657,12 +657,13 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } - const auto object_pose = obstacle_cruise_utils::getCurrentObjectPose( - predicted_object, predicted_objects.header, current_time, false); + const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPose( + predicted_object, predicted_objects.header, current_time, true); const auto & object_velocity = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; - const bool is_front_obstacle = isFrontObstacle(traj, ego_idx, object_pose.pose.position); + const bool is_front_obstacle = + isFrontObstacle(traj, ego_idx, current_object_pose.pose.position); if (!is_front_obstacle) { RCLCPP_INFO_EXPRESSION( get_logger(), is_showing_debug_info_, @@ -672,7 +673,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // rough detection area filtering without polygons const double dist_from_obstacle_to_traj = [&]() { - return motion_utils::calcLateralOffset(decimated_traj.points, object_pose.pose.position); + return motion_utils::calcLateralOffset( + decimated_traj.points, current_object_pose.pose.position); }(); const double obstacle_max_length = calcObjectMaxLength(predicted_object.shape); if ( @@ -685,26 +687,29 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } - // calculate collision points - const auto obstacle_polygon = - tier4_autoware_utils::toPolygon2d(object_pose.pose, predicted_object.shape); + // Get highest confidence predicted path + const auto predicted_path = getHighestConfidencePredictedPath(predicted_object); + const auto resampled_predicted_path = perception_utils::resamplePredictedPath( + predicted_path, obstacle_filtering_param_.prediction_resampling_time_interval, + obstacle_filtering_param_.prediction_resampling_time_horizon); + + // calculate current collision points std::vector closest_collision_points; - const auto first_within_idx = polygon_utils::getFirstCollisionIndex( - decimated_traj_polygons, obstacle_polygon, predicted_objects.header, + const auto first_within_idx = polygon_utils::getCollisionIndex( + decimated_traj, decimated_traj_polygons, current_object_pose, predicted_object.shape, closest_collision_points); // precise detection area filtering with polygons std::vector collision_points; if (first_within_idx) { // obstacles inside the trajectory // calculate nearest collision point - collision_points = calcNearestCollisionPoint( - first_within_idx.get(), closest_collision_points, decimated_traj, is_driving_forward); - if (!collision_points.empty()) { - debug_data.collision_points.push_back(collision_points.front().point); - } + collision_points = polygon_utils::getCollisionPoints( + decimated_traj, decimated_traj_polygons, predicted_objects.header, resampled_predicted_path, + predicted_object.shape, current_time, vehicle_info_.max_longitudinal_offset_m, + is_driving_forward); const bool is_angle_aligned = isAngleAlignedWithTrajectory( - decimated_traj, object_pose.pose, + decimated_traj, current_object_pose.pose, obstacle_filtering_param_.crossing_obstacle_traj_angle_threshold); const double has_high_speed = std::abs(object_velocity) > obstacle_filtering_param_.crossing_obstacle_velocity_threshold; @@ -712,8 +717,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( // ignore running vehicle crossing the ego trajectory with high speed with some condition if (!is_angle_aligned && has_high_speed && !collision_points.empty()) { const double collision_time_margin = calcCollisionTimeMargin( - current_pose, current_vel, collision_points.front().point, predicted_object, - first_within_idx.get(), decimated_traj, decimated_traj_polygons, is_driving_forward); + current_pose, current_vel, collision_points, predicted_object, decimated_traj, + is_driving_forward); if (collision_time_margin > obstacle_filtering_param_.collision_time_margin) { // Ignore vehicle obstacles inside the trajectory, which is crossing the trajectory with // high speed and does not collide with ego in a certain time. @@ -756,20 +761,15 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } - const auto predicted_path = getHighestConfidencePredictedPath(predicted_object); - const auto resampled_predicted_path = perception_utils::resamplePredictedPath( - predicted_path, obstacle_filtering_param_.prediction_resampling_time_interval, - obstacle_filtering_param_.prediction_resampling_time_horizon); - - std::vector future_collision_points; - const auto collision_traj_poly_idx = polygon_utils::willCollideWithSurroundObstacle( + collision_points = polygon_utils::willCollideWithSurroundObstacle( decimated_traj, decimated_traj_polygons, predicted_objects.header, resampled_predicted_path, - predicted_object.shape, + predicted_object.shape, current_time, vehicle_info_.vehicle_width_m + obstacle_filtering_param_.rough_detection_area_expand_width, obstacle_filtering_param_.ego_obstacle_overlap_time_threshold, - obstacle_filtering_param_.max_prediction_time_for_collision_check, future_collision_points); + obstacle_filtering_param_.max_prediction_time_for_collision_check, + vehicle_info_.max_longitudinal_offset_m, is_driving_forward); - if (!collision_traj_poly_idx) { + if (collision_points.empty()) { // Ignore vehicle obstacles outside the trajectory, whose predicted path // overlaps the ego trajectory in a certain time. RCLCPP_INFO_EXPRESSION( @@ -779,12 +779,11 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( debug_data.intentionally_ignored_obstacles.push_back(predicted_object); continue; } + } - collision_points = calcNearestCollisionPoint( - collision_traj_poly_idx.get(), future_collision_points, decimated_traj, is_driving_forward); - if (!collision_points.empty()) { - debug_data.collision_points.push_back(collision_points.front().point); - } + // For debug + for (const auto & cp : collision_points) { + debug_data.collision_points.push_back(cp.point); } // convert to obstacle type @@ -910,56 +909,12 @@ void ObstacleCruisePlannerNode::checkConsistency( } } -std::vector ObstacleCruisePlannerNode::calcNearestCollisionPoint( - const size_t & first_within_idx, - const std::vector & collision_points, - const Trajectory & decimated_traj, const bool is_driving_forward) -{ - std::array segment_points; - if (first_within_idx == 0) { - const auto & traj_front_pose = decimated_traj.points.at(0).pose; - const auto front_pos = tier4_autoware_utils::calcOffsetPose( - traj_front_pose, vehicle_info_.max_longitudinal_offset_m, 0.0, 0.0) - .position; - if (is_driving_forward) { - segment_points.at(0) = traj_front_pose.position; - segment_points.at(1) = front_pos; - } else { - segment_points.at(0) = front_pos; - segment_points.at(1) = traj_front_pose.position; - } - } else { - const size_t seg_idx = first_within_idx - 1; - segment_points.at(0) = decimated_traj.points.at(seg_idx).pose.position; - segment_points.at(1) = decimated_traj.points.at(seg_idx + 1).pose.position; - } - - size_t min_idx = 0; - double min_dist = std::numeric_limits::max(); - for (size_t cp_idx = 0; cp_idx < collision_points.size(); ++cp_idx) { - const auto & collision_point = collision_points.at(cp_idx); - const double dist = - motion_utils::calcLongitudinalOffsetToSegment(segment_points, 0, collision_point.point); - if (dist < min_dist) { - min_dist = dist; - min_idx = cp_idx; - } - } - - return {collision_points.at(min_idx)}; -} - double ObstacleCruisePlannerNode::calcCollisionTimeMargin( const geometry_msgs::msg::Pose & current_pose, const double current_vel, - const geometry_msgs::msg::Point & nearest_collision_point, - const PredictedObject & predicted_object, const size_t first_within_idx, - const Trajectory & decimated_traj, - const std::vector & decimated_traj_polygons, + const std::vector & collision_points, + const PredictedObject & predicted_object, const Trajectory & decimated_traj, const bool is_driving_forward) { - const auto & object_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; - const auto & object_velocity = - predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; const auto predicted_path = getHighestConfidencePredictedPath(predicted_object); const auto resampled_predicted_path = perception_utils::resamplePredictedPath( predicted_path, obstacle_filtering_param_.prediction_resampling_time_interval, @@ -971,23 +926,14 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( : std::abs(vehicle_info_.min_longitudinal_offset_m); const double dist_from_ego_to_obstacle = motion_utils::calcSignedArcLength( - decimated_traj.points, current_pose.position, nearest_collision_point) - + decimated_traj.points, current_pose.position, collision_points.front().point) - abs_ego_offset; return dist_from_ego_to_obstacle / std::max(1e-6, std::abs(current_vel)); }(); - const double time_to_obstacle_getting_out = [&]() { - const auto obstacle_getting_out_idx = polygon_utils::getFirstNonCollisionIndex( - decimated_traj_polygons, resampled_predicted_path, predicted_object.shape, first_within_idx); - if (!obstacle_getting_out_idx) { - return std::numeric_limits::max(); - } - - const double dist_to_obstacle_getting_out = motion_utils::calcSignedArcLength( - decimated_traj.points, object_pose.position, obstacle_getting_out_idx.get()); - - return dist_to_obstacle_getting_out / object_velocity; - }(); + const double time_to_obstacle_getting_out = (rclcpp::Time(collision_points.back().header.stamp) - + rclcpp::Time(collision_points.front().header.stamp)) + .seconds(); return time_to_collision - time_to_obstacle_getting_out; } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 9166bec372e55..8302736cb6149 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -102,7 +102,7 @@ Trajectory PlannerInterface::generateStopTrajectory( // Get Closest Stop Obstacle const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj, planner_data.target_obstacles); - if (!closest_stop_obstacle && closest_stop_obstacle->collision_points.empty()) { + if (!closest_stop_obstacle || closest_stop_obstacle->collision_points.empty()) { // delete marker const auto markers = motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 7bd486306f680..7d63199ce2fa2 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -14,6 +14,7 @@ #include "obstacle_cruise_planner/polygon_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" namespace @@ -86,13 +87,21 @@ Polygon2d createOneStepPolygon( namespace polygon_utils { -boost::optional getFirstCollisionIndex( - const std::vector & traj_polygons, const Polygon2d & obj_polygon, - const std_msgs::msg::Header & obj_header, - std::vector & collision_geom_points) +boost::optional getCollisionIndex( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & collision_geom_points, const double max_dist) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose.pose, shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { - std::deque collision_polygons; + const double approximated_dist = + tier4_autoware_utils::calcDistance2d(traj.points.at(i).pose, obj_pose.pose); + if (approximated_dist > max_dist) { + continue; + } + + std::vector collision_polygons; boost::geometry::intersection(traj_polygons.at(i), obj_polygon, collision_polygons); bool has_collision = false; @@ -102,7 +111,7 @@ boost::optional getFirstCollisionIndex( for (const auto & collision_point : collision_polygon.outer()) { geometry_msgs::msg::PointStamped collision_geom_point; - collision_geom_point.header = obj_header; + collision_geom_point.header = obj_pose.header; collision_geom_point.point.x = collision_point.x(); collision_geom_point.point.y = collision_point.y(); collision_geom_points.push_back(collision_geom_point); @@ -118,106 +127,74 @@ boost::optional getFirstCollisionIndex( return {}; } -boost::optional getFirstNonCollisionIndex( - const std::vector & traj_polygons, +std::vector getCollisionPoints( + const autoware_auto_planning_msgs::msg::Trajectory & traj, + const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const size_t start_idx) + const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const double vehicle_max_longitudinal_offset, const bool is_driving_forward, + const double max_dist, const double max_prediction_time_for_collision_check) { - constexpr double epsilon = 1e-3; - - size_t latest_collision_idx = start_idx; - for (const auto & path_point : predicted_path.path) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(path_point, shape); - for (size_t i = start_idx; i < traj_polygons.size(); ++i) { - const double dist = bg::distance(traj_polygons.at(i), obj_polygon); - if (dist <= epsilon) { - latest_collision_idx = i; - break; - } - if (i == traj_polygons.size() - 1) { - return latest_collision_idx; - } + std::vector collision_points; + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + if ( + max_prediction_time_for_collision_check < + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) { + break; + } + + const auto object_time = + rclcpp::Time(obj_header.stamp) + rclcpp::Duration(predicted_path.time_step) * i; + // Ignore past position + if ((object_time - current_time).seconds() < 0.0) { + continue; + } + + geometry_msgs::msg::PoseStamped obj_pose; + obj_pose.header.frame_id = obj_header.frame_id; + obj_pose.header.stamp = object_time; + obj_pose.pose = predicted_path.path.at(i); + + std::vector current_collision_points; + const auto collision_idx = + getCollisionIndex(traj, traj_polygons, obj_pose, shape, current_collision_points, max_dist); + if (collision_idx) { + const auto nearest_collision_point = calcNearestCollisionPoint( + *collision_idx, current_collision_points, traj, vehicle_max_longitudinal_offset, + is_driving_forward); + collision_points.push_back(nearest_collision_point); } } - return {}; + + return collision_points; } -boost::optional willCollideWithSurroundObstacle( +std::vector willCollideWithSurroundObstacle( const autoware_auto_planning_msgs::msg::Trajectory & traj, const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const double max_dist, - const double ego_obstacle_overlap_time_threshold, + const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const double max_dist, const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, - std::vector & collision_geom_points) + const double vehicle_max_longitudinal_offset, const bool is_driving_forward) { - constexpr double epsilon = 1e-3; + const auto collision_points = getCollisionPoints( + traj, traj_polygons, obj_header, predicted_path, shape, current_time, + vehicle_max_longitudinal_offset, is_driving_forward, max_dist, + max_prediction_time_for_collision_check); - bool is_found = false; - size_t start_predicted_path_idx = 0; - for (size_t i = 0; i < predicted_path.path.size(); ++i) { - const auto & path_point = predicted_path.path.at(i); - if ( - max_prediction_time_for_collision_check < - rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) { - return {}; - } - - for (size_t j = 0; j < traj.points.size(); ++j) { - const auto & traj_point = traj.points.at(j); - const double approximated_dist = - tier4_autoware_utils::calcDistance2d(path_point.position, traj_point.pose.position); - if (approximated_dist > max_dist) { - continue; - } + if (collision_points.empty()) { + return {}; + } - const auto & traj_polygon = traj_polygons.at(j); - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(path_point, shape); - const double dist = bg::distance(traj_polygon, obj_polygon); - - if (dist < epsilon) { - if (!is_found) { - // calculate collision point by polygon collision - std::deque collision_polygons; - boost::geometry::intersection(traj_polygon, obj_polygon, collision_polygons); - - bool has_collision = false; - for (const auto & collision_polygon : collision_polygons) { - if (boost::geometry::area(collision_polygon) > 0.0) { - has_collision = true; - - for (const auto & collision_point : collision_polygon.outer()) { - geometry_msgs::msg::PointStamped collision_geom_point; - const auto collision_time = - rclcpp::Time(obj_header.stamp) + rclcpp::Duration(predicted_path.time_step) * i; - collision_geom_point.header.frame_id = obj_header.frame_id; - collision_geom_point.header.stamp = collision_time; - collision_geom_point.point.x = collision_point.x(); - collision_geom_point.point.y = collision_point.y(); - collision_geom_points.push_back(collision_geom_point); - } - } - } - - if (has_collision) { - start_predicted_path_idx = i; - is_found = true; - } - } else { - const double overlap_time = (static_cast(i) - start_predicted_path_idx) * - rclcpp::Duration(predicted_path.time_step).seconds(); - if (ego_obstacle_overlap_time_threshold < overlap_time) { - return j; - } - } - } else { - is_found = false; - } - } + const double overlap_time = (rclcpp::Time(collision_points.back().header.stamp) - + rclcpp::Time(collision_points.front().header.stamp)) + .seconds(); + if (overlap_time < ego_obstacle_overlap_time_threshold) { + return {}; } - collision_geom_points.clear(); - return {}; + return collision_points; } std::vector createOneStepPolygons( @@ -240,4 +217,44 @@ std::vector createOneStepPolygons( } return polygons; } + +geometry_msgs::msg::PointStamped calcNearestCollisionPoint( + const size_t & first_within_idx, + const std::vector & collision_points, + const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, + const double vehicle_max_longitudinal_offset, const bool is_driving_forward) +{ + std::array segment_points; + if (first_within_idx == 0) { + const auto & traj_front_pose = decimated_traj.points.at(0).pose; + const auto front_pos = tier4_autoware_utils::calcOffsetPose( + traj_front_pose, vehicle_max_longitudinal_offset, 0.0, 0.0) + .position; + if (is_driving_forward) { + segment_points.at(0) = traj_front_pose.position; + segment_points.at(1) = front_pos; + } else { + segment_points.at(0) = front_pos; + segment_points.at(1) = traj_front_pose.position; + } + } else { + const size_t seg_idx = first_within_idx - 1; + segment_points.at(0) = decimated_traj.points.at(seg_idx).pose.position; + segment_points.at(1) = decimated_traj.points.at(seg_idx + 1).pose.position; + } + + size_t min_idx = 0; + double min_dist = std::numeric_limits::max(); + for (size_t cp_idx = 0; cp_idx < collision_points.size(); ++cp_idx) { + const auto & collision_point = collision_points.at(cp_idx); + const double dist = + motion_utils::calcLongitudinalOffsetToSegment(segment_points, 0, collision_point.point); + if (dist < min_dist) { + min_dist = dist; + min_idx = cp_idx; + } + } + + return collision_points.at(min_idx); +} } // namespace polygon_utils diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 821ff59e79bad..e194434de228a 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -4,9 +4,14 @@ obstacle_stop_planner 0.1.0 The obstacle_stop_planner package - Yukihiro Saito + + Satoshi Ota + Apache License 2.0 + Satoshi Ota + Yukihiro Saito + ament_cmake_auto autoware_cmake diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index f6d367b25e9b5..f41a97d8ff924 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -1392,14 +1392,18 @@ bool ObstacleStopPlannerNode::searchPointcloudNearTrajectory( ? slow_down_param_.slow_down_search_radius : stop_param.stop_search_radius; const double squared_radius = search_radius * search_radius; - for (const auto & trajectory_point : trajectory) { - const auto center_pose = getVehicleCenterFromBase(trajectory_point.pose, vehicle_info); - for (const auto & point : transformed_points_ptr->points) { - const double x = center_pose.position.x - point.x; - const double y = center_pose.position.y - point.y; + std::vector center_points; + center_points.reserve(trajectory.size()); + for (const auto & trajectory_point : trajectory) + center_points.push_back(getVehicleCenterFromBase(trajectory_point.pose, vehicle_info).position); + for (const auto & point : transformed_points_ptr->points) { + for (const auto & center_point : center_points) { + const double x = center_point.x - point.x; + const double y = center_point.y - point.y; const double squared_distance = x * x + y * y; if (squared_distance < squared_radius) { output_points_ptr->points.push_back(point); + break; } } } diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index f4897bb57c609..55e20cad2e05d 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -4,9 +4,11 @@ planning_debug_tools 0.1.0 The planning_debug_tools package - Takamasa Horibe + Takamasa Horibe Apache License 2.0 + Takamasa Horibe + ament_cmake_auto eigen3_cmake_module diff --git a/planning/planning_error_monitor/package.xml b/planning/planning_error_monitor/package.xml index e667c81790875..c0f2347ae40a6 100644 --- a/planning/planning_error_monitor/package.xml +++ b/planning/planning_error_monitor/package.xml @@ -7,6 +7,8 @@ Yutaka Shimizu Apache License 2.0 + Yutaka Shimizu + ament_cmake_auto autoware_cmake diff --git a/planning/planning_evaluator/package.xml b/planning/planning_evaluator/package.xml index 2f427e79e4d39..b288d911b4c3c 100644 --- a/planning/planning_evaluator/package.xml +++ b/planning/planning_evaluator/package.xml @@ -7,6 +7,8 @@ Maxime CLEMENT Apache License 2.0 + Maxime CLEMENT + ament_cmake_auto autoware_cmake diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 7a9b20103ec93..92737ca586fc1 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -252,9 +252,9 @@ class RouteHandler const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; bool getPullOverTarget( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; - bool getPullOutStart( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, - const Pose & pose, const double vehicle_width) const; + bool getPullOutStartLane( + const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, + lanelet::ConstLanelet * target_lanelet) const; double getLaneChangeableDistance( const Pose & current_pose, const LaneChangeDirection & direction) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index f83872e6eabcd..b3db23556dcd0 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -7,6 +7,8 @@ Fumiya Watanabe Apache License 2.0 + Fumiya Watanabe + ament_cmake_auto autoware_cmake diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 288669a21c2a0..1ed082216d3bf 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1141,9 +1141,9 @@ bool RouteHandler::getPullOverTarget( return false; } -bool RouteHandler::getPullOutStart( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, - const Pose & pose, const double vehicle_width) const +bool RouteHandler::getPullOutStartLane( + const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, + lanelet::ConstLanelet * target_lanelet) const { for (const auto & shoulder_lanelet : lanelets) { if (lanelet::utils::isInLanelet(pose, shoulder_lanelet, vehicle_width / 2.0)) { diff --git a/planning/rtc_auto_approver/package.xml b/planning/rtc_auto_approver/package.xml index e8b7793c3cd7a..668e5b8b58893 100644 --- a/planning/rtc_auto_approver/package.xml +++ b/planning/rtc_auto_approver/package.xml @@ -9,6 +9,8 @@ Apache License 2.0 + Fumiya Watanabe + ament_cmake_auto autoware_cmake diff --git a/planning/rtc_interface/package.xml b/planning/rtc_interface/package.xml index 96293f6b48ff3..83853332884c2 100644 --- a/planning/rtc_interface/package.xml +++ b/planning/rtc_interface/package.xml @@ -8,6 +8,8 @@ Apache License 2.0 + Fumiya Watanabe + ament_cmake_auto rclcpp diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index fd03a8c394f18..e63489d80cc83 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -7,6 +7,8 @@ Kenji Miyake Apache License 2.0 + Kenji Miyake + ament_cmake_auto autoware_cmake diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 680c8213fa9f8..243cf2cf3dfd1 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -7,6 +7,8 @@ Satoshi Ota Apache License 2.0 + Satoshi Ota + ament_cmake eigen3_cmake_module diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index bd7a05ee0f9bd..6c51755768320 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -45,6 +45,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `angle_range` | vector | The effective range of LiDAR | | `vertical_bins` | int | The LiDAR channel number | | `model` | string | The LiDAR model | +| `buffering_frames` | uint | The number of buffering [range:1-200] | +| `buffering_interval` | uint | The interval of buffering | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg b/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg index cfa45e9761d28..b560e699dc273 100644 --- a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg +++ b/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg @@ -1,4 +1,4 @@ -
Depth map
Depth map
no return region detection
no return region det...
small segment filter
small segment filter
ground region blockage extract
ground region blocka...input point clouddiagnosticsblockage maskText is not SVG - cannot display \ No newline at end of file +
input pointcloud
input pointcloud
Depth map
Depth map
no retrurn region 
detection
no retrurn region...
small segment
fillter
small segment...
buffering
buffering
extract no return region in (multi) frames
extract no return regi...
sky-range and
ground range
comparison
sky-range and...
blockage mask
blockage mask
diagnotics
diagnoticsText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 6e7a9b3454e74..54fb4c2fa2307 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -69,6 +69,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter uint sky_blockage_count_ = 0; uint blockage_count_threshold_; std::string lidar_model_; + uint buffering_frames_ = 100; + uint buffering_interval_ = 5; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 3f9f427a8f93d..0f740eee6be40 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -16,7 +16,7 @@ #include "autoware_point_types/types.hpp" -#include +#include #include @@ -38,6 +38,8 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options lidar_model_ = static_cast(declare_parameter("model", "Pandar40P")); blockage_count_threshold_ = static_cast(declare_parameter("blockage_count_threshold", 50)); + buffering_frames_ = static_cast(declare_parameter("buffering_frames", 100)); + buffering_interval_ = static_cast(declare_parameter("buffering_interval", 5)); } updater_.setHardwareID("blockage_diag"); @@ -73,7 +75,6 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) stat.add( "sky_blockage_range_deg", "[" + std::to_string(sky_blockage_range_deg_[0]) + "," + std::to_string(sky_blockage_range_deg_[1]) + "]"); - // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo] auto level = DiagnosticStatus::OK; @@ -151,16 +152,44 @@ void BlockageDiagComponent::filter( lidar_depth_map.convertTo(lidar_depth_map, CV_8UC1, 1.0 / 100.0); cv::Mat no_return_mask; cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); + cv::Mat erosion_dst; cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1), cv::Point(erode_kernel_, erode_kernel_)); cv::erode(no_return_mask, erosion_dst, element); cv::dilate(erosion_dst, no_return_mask, element); + + static boost::circular_buffer no_return_mask_buffer(buffering_frames_); + + cv::Mat no_return_mask_result(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat time_series_blockage_mask( + cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat no_return_mask_binarized( + cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + + static uint frame_count; + frame_count++; + if (buffering_interval_ != 0) { + no_return_mask_binarized = no_return_mask / 255; + if (frame_count == buffering_interval_) { + no_return_mask_buffer.push_back(no_return_mask_binarized); + frame_count = 0; + } + for (const auto & binary_mask : no_return_mask_buffer) { + time_series_blockage_mask += binary_mask; + } + cv::inRange( + time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), + no_return_mask_result); + } else { + no_return_mask.copyTo(no_return_mask_result); + } cv::Mat ground_no_return_mask; cv::Mat sky_no_return_mask; - no_return_mask(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)).copyTo(sky_no_return_mask); - no_return_mask( + no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)) + .copyTo(sky_no_return_mask); + no_return_mask_result( cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) .copyTo(ground_no_return_mask); ground_blockage_ratio_ = @@ -203,7 +232,7 @@ void BlockageDiagComponent::filter( lidar_depth_map_pub_.publish(lidar_depth_msg); cv::Mat blockage_mask_colorized; - cv::applyColorMap(no_return_mask, blockage_mask_colorized, cv::COLORMAP_JET); + cv::applyColorMap(no_return_mask_result, blockage_mask_colorized, cv::COLORMAP_JET); sensor_msgs::msg::Image::SharedPtr blockage_mask_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); blockage_mask_msg->header = input->header; @@ -249,6 +278,12 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( get_logger(), " Setting new angle_range to: [%f , %f].", angle_range_deg_[0], angle_range_deg_[1]); } + if (get_param(p, "buffering_frames", buffering_frames_)) { + RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_); + } + if (get_param(p, "buffering_interval", buffering_interval_)) { + RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success"; diff --git a/sensing/vehicle_velocity_converter/README.md b/sensing/vehicle_velocity_converter/README.md index e25c46dfd7c0d..7554457d37149 100644 --- a/sensing/vehicle_velocity_converter/README.md +++ b/sensing/vehicle_velocity_converter/README.md @@ -20,7 +20,8 @@ This package converts autoware_auto_vehicle_msgs::msg::VehicleReport message to ## Parameters -| Name | Type | Description | -| ------------ | ------ | ----------------------------------------- | -| `frame_id` | string | frame id for output message | -| `covariance` | double | set covariance value to the twist message | +| Name | Type | Description | +| ---------------------------- | ------ | ------------------------------- | +| `frame_id` | string | frame id for output message | +| `velocity_stddev_xx` | double | standard deviation for vx | +| `angular_velocity_stddev_zz` | double | standard deviation for yaw rate | diff --git a/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml b/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml index 4c15d1caf6274..7dd04823453c5 100644 --- a/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml +++ b/sensing/vehicle_velocity_converter/config/vehicle_velocity_converter.param.yaml @@ -1,12 +1,5 @@ /**: ros__parameters: + velocity_stddev_xx: 0.2 # [m/s] + angular_velocity_stddev_zz: 0.1 # [rad/s] frame_id: base_link - twist_covariance: - [ - 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 10000.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 10000.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 10000.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 10000.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, - ] diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 043087a3afdf2..bad4d4bc06326 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -41,6 +41,8 @@ class VehicleVelocityConverter : public rclcpp::Node twist_with_covariance_pub_; std::string frame_id_; + double stddev_vx_; + double stddev_wz_; std::array twist_covariance_; }; diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index d4cc91a2b0ae9..844a17df4715b 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -17,10 +17,8 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_converter") { // set covariance value for twist with covariance msg - std::vector covariance = declare_parameter>("twist_covariance"); - for (std::size_t i = 0; i < covariance.size(); ++i) { - twist_covariance_[i] = covariance[i]; - } + stddev_vx_ = declare_parameter("velocity_stddev_xx", 0.2); + stddev_wz_ = declare_parameter("angular_velocity_stddev_zz", 0.1); frame_id_ = declare_parameter("frame_id", "base_link"); vehicle_report_sub_ = create_subscription( @@ -44,7 +42,12 @@ void VehicleVelocityConverter::callbackVelocityReport( twist_with_covariance_msg.twist.twist.linear.x = msg->longitudinal_velocity; twist_with_covariance_msg.twist.twist.linear.y = msg->lateral_velocity; twist_with_covariance_msg.twist.twist.angular.z = msg->heading_rate; - twist_with_covariance_msg.twist.covariance = twist_covariance_; + twist_with_covariance_msg.twist.covariance[0 + 0 * 6] = stddev_vx_ * stddev_vx_; + twist_with_covariance_msg.twist.covariance[1 + 1 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[2 + 2 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[3 + 3 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[4 + 4 * 6] = 10000.0; + twist_with_covariance_msg.twist.covariance[5 + 5 * 6] = stddev_wz_ * stddev_wz_; twist_with_covariance_pub_->publish(twist_with_covariance_msg); } diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 3a087c5d60f2f..b936b1a0600c1 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -4,7 +4,7 @@ simple_planning_simulator 1.0.0 simple_planning_simulator as a ROS 2 node - Takamasa HORIBE + Takamasa Horibe Apache License 2.0 ament_cmake_auto diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index e3f60904c24fa..45cdbf1464129 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,11 +6,13 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/interface.cpp + src/localization.cpp src/routing.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::InterfaceNode" + "default_ad_api::LocalizationNode" "default_ad_api::RoutingNode" ) 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 7e11805870c5b..d93f779427277 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -31,6 +31,7 @@ def _create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): components = [ _create_api_node("interface", "InterfaceNode"), + _create_api_node("localization", "LocalizationNode"), _create_api_node("routing", "RoutingNode"), ] container = ComposableNodeContainer( diff --git a/system/default_ad_api/src/localization.cpp b/system/default_ad_api/src/localization.cpp new file mode 100644 index 0000000000000..468e02b37644f --- /dev/null +++ b/system/default_ad_api/src/localization.cpp @@ -0,0 +1,32 @@ +// 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 "localization.hpp" + +namespace default_ad_api +{ + +LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options) +: Node("localization", options) +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.relay_message(pub_state_, sub_state_); + adaptor.relay_service(cli_initialize_, srv_initialize_, group_cli_); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::LocalizationNode) diff --git a/system/default_ad_api/src/localization.hpp b/system/default_ad_api/src/localization.hpp new file mode 100644 index 0000000000000..077c60d86df01 --- /dev/null +++ b/system/default_ad_api/src/localization.hpp @@ -0,0 +1,44 @@ +// 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 LOCALIZATION_HPP_ +#define LOCALIZATION_HPP_ + +#include +#include +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class LocalizationNode : public rclcpp::Node +{ +public: + explicit LocalizationNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + Srv srv_initialize_; + Pub pub_state_; + Cli cli_initialize_; + Sub sub_state_; +}; + +} // namespace default_ad_api + +#endif // LOCALIZATION_HPP_ diff --git a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt b/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt index 65028d9ba5cc5..35cb599995b31 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt +++ b/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt @@ -4,8 +4,12 @@ project(ad_api_adaptors) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_executable(initial_pose_adaptor + src/initial_pose_adaptor.cpp +) + ament_auto_add_executable(routing_adaptor src/routing_adaptor.cpp ) -ament_auto_package(INSTALL_TO_SHARE launch) +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index bb25903043bac..95cd40a2ec33b 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -1,5 +1,17 @@ # ad_api_adaptors +## initial_pose_adaptor + +This node makes it easy to use the localization AD API from RViz. +When a initial pose topic is received, call the localization initialize API. +This node depends on fitting to map height service. + +| Interface | Local Name | Global Name | Description | +| ------------ | -------------- | --------------------------------- | ----------------------------------------- | +| Subscription | initialpose | /initialpose | The pose for localization initialization. | +| Client | fit_map_height | /localization/util/fit_map_height | To fix initial pose to map height | +| Client | - | /api/localization/initialize | The localization initialize API. | + ## routing_adaptor This node makes it easy to use the routing AD API from RViz. diff --git a/system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml b/system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml new file mode 100644 index 0000000000000..46748a022f558 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + + # from initialpose (Rviz's 2DPoseEstimate) + initial_pose_particle_covariance: + [ + 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 4.0, 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, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, + ] diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml index 976d0868fe20e..b3301a3c30f8e 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml @@ -1,6 +1,14 @@ - - - - + + + + + + + + + + + + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index b98d230a4c3cf..442a076a9c544 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -15,6 +15,7 @@ autoware_ad_api_specs component_interface_utils rclcpp + tier4_localization_msgs ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp new file mode 100644 index 0000000000000..137243baaf0d2 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp @@ -0,0 +1,72 @@ +// 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 "initial_pose_adaptor.hpp" + +#include +#include +#include + +namespace ad_api_adaptors +{ +template +using Future = typename rclcpp::Client::SharedFuture; + +std::array get_covariance_parameter(rclcpp::Node * node, const std::string & name) +{ + const auto vector = node->declare_parameter>(name); + if (vector.size() != 36) { + throw std::invalid_argument("The covariance parameter size is not 36."); + } + std::array array; + std::copy_n(vector.begin(), array.size(), array.begin()); + return array; +} + +InitialPoseAdaptor::InitialPoseAdaptor() : Node("initial_pose_adaptor") +{ + rviz_particle_covariance_ = get_covariance_parameter(this, "initial_pose_particle_covariance"); + cli_map_fit_ = create_client("~/fit_map_height"); + sub_initial_pose_ = create_subscription( + "~/initialpose", rclcpp::QoS(1), + std::bind(&InitialPoseAdaptor::on_initial_pose, this, std::placeholders::_1)); + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_cli(cli_initialize_); +} + +void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->pose_with_covariance = *msg; + cli_map_fit_->async_send_request(req, [this](Future future) { + const auto req = std::make_shared(); + req->pose.push_back(future.get()->pose_with_covariance); + req->pose.back().pose.covariance = rviz_particle_covariance_; + cli_initialize_->async_send_request(req); + }); +} + +} // namespace ad_api_adaptors + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp new file mode 100644 index 0000000000000..c23968acc4ed9 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp @@ -0,0 +1,47 @@ +// 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 INITIAL_POSE_ADAPTOR_HPP_ +#define INITIAL_POSE_ADAPTOR_HPP_ + +#include +#include +#include + +#include +#include + +namespace ad_api_adaptors +{ + +class InitialPoseAdaptor : public rclcpp::Node +{ +public: + InitialPoseAdaptor(); + +private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using Initialize = autoware_ad_api::localization::Initialize; + using RequestHeightFitting = tier4_localization_msgs::srv::PoseWithCovarianceStamped; + rclcpp::Subscription::SharedPtr sub_initial_pose_; + rclcpp::Client::SharedPtr cli_map_fit_; + component_interface_utils::Client::SharedPtr cli_initialize_; + std::array rviz_particle_covariance_; + + void on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg); +}; + +} // namespace ad_api_adaptors + +#endif // INITIAL_POSE_ADAPTOR_HPP_ diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt b/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt new file mode 100644 index 0000000000000..ad65f04356f4d --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(automatic_pose_initializer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(automatic_pose_initializer + src/automatic_pose_initializer.cpp +) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/README.md b/system/default_ad_api_helpers/automatic_pose_initializer/README.md new file mode 100644 index 0000000000000..4fda3706356d3 --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/README.md @@ -0,0 +1,11 @@ +# automatic_pose_initializer + +## automatic_pose_initializer + +This node calls localization initialize API when the localization initialization state is uninitialized. +Since the API uses GNSS pose when no pose is specified, initialization using GNSS can be performed automatically. + +| Interface | Local Name | Global Name | Description | +| ------------ | ---------- | -------------------------------------- | ------------------------------------------ | +| Subscription | - | /api/localization/initialization_state | The localization initialization state API. | +| Client | - | /api/localization/initialize | The localization initialize API. | diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml b/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml new file mode 100644 index 0000000000000..ac5e63e84092a --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml new file mode 100644 index 0000000000000..e6a351b392680 --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -0,0 +1,25 @@ + + + + automatic_pose_initializer + 0.1.0 + The automatic_pose_initializer package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_ad_api_msgs + autoware_ad_api_specs + component_interface_utils + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp new file mode 100644 index 0000000000000..6970d489ff340 --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp @@ -0,0 +1,60 @@ +// 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 "automatic_pose_initializer.hpp" + +#include + +namespace automatic_pose_initializer +{ + +AutomaticPoseInitializer::AutomaticPoseInitializer() : Node("automatic_pose_initializer") +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_cli(cli_initialize_, group_cli_); + adaptor.init_sub(sub_state_, [this](const State::Message::ConstSharedPtr msg) { state_ = *msg; }); + + const auto period = rclcpp::Rate(1.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); }); + + state_.stamp = now(); + state_.state = State::Message::UNKNOWN; +} + +void AutomaticPoseInitializer::on_timer() +{ + timer_->cancel(); + if (state_.state == State::Message::UNINITIALIZED) { + try { + const auto req = std::make_shared(); + cli_initialize_->call(req); + } catch (const component_interface_utils::ServiceException & error) { + } + } + timer_->reset(); +} + +} // namespace automatic_pose_initializer + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp new file mode 100644 index 0000000000000..80fe78832026d --- /dev/null +++ b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp @@ -0,0 +1,43 @@ +// 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 AUTOMATIC_POSE_INITIALIZER_HPP_ +#define AUTOMATIC_POSE_INITIALIZER_HPP_ + +#include +#include +#include + +namespace automatic_pose_initializer +{ + +class AutomaticPoseInitializer : public rclcpp::Node +{ +public: + AutomaticPoseInitializer(); + +private: + void on_timer(); + using Initialize = autoware_ad_api::localization::Initialize; + using State = autoware_ad_api::localization::InitializationState; + rclcpp::CallbackGroup::SharedPtr group_cli_; + rclcpp::TimerBase::SharedPtr timer_; + component_interface_utils::Client::SharedPtr cli_initialize_; + component_interface_utils::Subscription::SharedPtr sub_state_; + State::Message state_; +}; + +} // namespace automatic_pose_initializer + +#endif // AUTOMATIC_POSE_INITIALIZER_HPP_ diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index af6b9ab8a64c2..290a272eb2605 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -122,6 +122,12 @@ contains: [": Network Traffic"] timeout: 3.0 + netowork_crc_error: + type: diagnostic_aggregator/GenericAnalyzer + path: network_crc_error + contains: [": Network CRC Error"] + timeout: 3.0 + storage: type: diagnostic_aggregator/AnalyzerGroup path: storage diff --git a/system/system_monitor/README.md b/system/system_monitor/README.md index 79f390a80d7e7..6a776a51bb6d0 100644 --- a/system/system_monitor/README.md +++ b/system/system_monitor/README.md @@ -53,28 +53,29 @@ Every topic is published in 1 minute interval. [Usage] ✓：Supported, -：Not supported -| Node | Message | Intel | arm64(tegra) | arm64(raspi) | Notes | -| --------------- | ---------------------- | :---: | :----------: | :----------: | ------------------------------------------------------------- | -| CPU Monitor | CPU Temperature | ✓ | ✓ | ✓ | | -| | CPU Usage | ✓ | ✓ | ✓ | | -| | CPU Load Average | ✓ | ✓ | ✓ | | -| | CPU Thermal Throttling | ✓ | - | ✓ | | -| | CPU Frequency | ✓ | ✓ | ✓ | Notification of frequency only, normally error not generated. | -| HDD Monitor | HDD Temperature | ✓ | ✓ | ✓ | | -| | HDD PowerOnHours | ✓ | ✓ | ✓ | | -| | HDD TotalDataWritten | ✓ | ✓ | ✓ | | -| | HDD Usage | ✓ | ✓ | ✓ | | -| Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | -| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | -| NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | -| Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | -| | High-load Proc[0-9] | ✓ | ✓ | ✓ | | -| | High-mem Proc[0-9] | ✓ | ✓ | ✓ | | -| GPU Monitor | GPU Temperature | ✓ | ✓ | - | | -| | GPU Usage | ✓ | ✓ | - | | -| | GPU Memory Usage | ✓ | - | - | | -| | GPU Thermal Throttling | ✓ | - | - | | -| | GPU Frequency | - | ✓ | - | | +| Node | Message | Intel | arm64(tegra) | arm64(raspi) | Notes | +| --------------- | ---------------------- | :---: | :----------: | :----------: | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| CPU Monitor | CPU Temperature | ✓ | ✓ | ✓ | | +| | CPU Usage | ✓ | ✓ | ✓ | | +| | CPU Load Average | ✓ | ✓ | ✓ | | +| | CPU Thermal Throttling | ✓ | - | ✓ | | +| | CPU Frequency | ✓ | ✓ | ✓ | Notification of frequency only, normally error not generated. | +| HDD Monitor | HDD Temperature | ✓ | ✓ | ✓ | | +| | HDD PowerOnHours | ✓ | ✓ | ✓ | | +| | HDD TotalDataWritten | ✓ | ✓ | ✓ | | +| | HDD Usage | ✓ | ✓ | ✓ | | +| Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | +| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | +| | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | +| NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | +| Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | +| | High-load Proc[0-9] | ✓ | ✓ | ✓ | | +| | High-mem Proc[0-9] | ✓ | ✓ | ✓ | | +| GPU Monitor | GPU Temperature | ✓ | ✓ | - | | +| | GPU Usage | ✓ | ✓ | - | | +| | GPU Memory Usage | ✓ | - | - | | +| | GPU Thermal Throttling | ✓ | - | - | | +| | GPU Frequency | - | ✓ | - | | ## ROS parameters diff --git a/system/system_monitor/config/net_monitor.param.yaml b/system/system_monitor/config/net_monitor.param.yaml index 953d32d788ccf..686ee349b0765 100644 --- a/system/system_monitor/config/net_monitor.param.yaml +++ b/system/system_monitor/config/net_monitor.param.yaml @@ -3,3 +3,5 @@ devices: ["*"] traffic_reader_port: 7636 monitor_program: "greengrass" + crc_error_check_duration: 1 + crc_error_count_threshold: 1 diff --git a/system/system_monitor/docs/ros_parameters.md b/system/system_monitor/docs/ros_parameters.md index 779297492e00a..044c1eb10a5d0 100644 --- a/system/system_monitor/docs/ros_parameters.md +++ b/system/system_monitor/docs/ros_parameters.md @@ -53,10 +53,12 @@ mem_monitor: net_monitor: -| Name | Type | Unit | Default | Notes | -| :--------- | :----------: | :-----: | :-----: | :----------------------------------------------------------------------------------- | -| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | -| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | +| Name | Type | Unit | Default | Notes | +| :------------------------ | :----------: | :-----: | :-----: | :-------------------------------------------------------------------------------------------------------------- | +| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | +| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | +| crc_error_check_duration | int | sec | 1 | CRC error check duration. | +| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | ## NTP Monitor diff --git a/system/system_monitor/docs/topics_net_monitor.md b/system/system_monitor/docs/topics_net_monitor.md index 36fdeea890e6a..261cede53de21 100644 --- a/system/system_monitor/docs/topics_net_monitor.md +++ b/system/system_monitor/docs/topics_net_monitor.md @@ -63,3 +63,21 @@ | key | value (example) | | ----- | ----------------------------------------------------- | | error | [nethogs -t] execve failed: No such file or directory | + +## Network CRC Error + +/diagnostics/net_monitor: Network CRC Error + +[summary] + +| level | message | +| ----- | ------- | +| OK | OK | + +[values] + +| key | value (example) | +| ------------------------------------------ | --------------- | +| Network [0-9]: interface name | wlp82s0 | +| Network [0-9]: total rx_crc_errors | 0 | +| Network [0-9]: rx_crc_errors per unit time | 0 | diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index 8aafb4b08b083..53f3e1250c9f0 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -55,11 +56,6 @@ class NetMonitor : public rclcpp::Node */ ~NetMonitor(); - /** - * @brief Update the diagnostic state. - */ - void update(); - /** * @brief Shutdown nl80211 object */ @@ -86,6 +82,15 @@ class NetMonitor : public rclcpp::Node void monitorTraffic( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief check CRC error + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkCrcError( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** * @brief get wireless speed * @param [in] ifa_name interface name @@ -93,17 +98,104 @@ class NetMonitor : public rclcpp::Node */ float getWirelessSpeed(const char * ifa_name); + /** + * @brief timer callback + */ + void onTimer(); + + /** + * @brief update Network information list + */ + void updateNetworkInfoList(); + + /** + * @brief check NetMonitor General Infomation + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @return check result + */ + bool checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** + * @brief Network information + */ + struct NetworkInfo + { + int mtu_errno; //!< @brief errno set by ioctl() with SIOCGIFMTU + int ethtool_errno; //!< @brief errno set by ioctl() with SIOCETHTOOL + bool is_running; //!< @brief resource allocated flag + std::string interface_name; //!< @brief interface name + float speed; //!< @brief network capacity + int mtu; //!< @brief MTU + float rx_traffic; //!< @brief traffic received + float tx_traffic; //!< @brief traffic transmitted + float rx_usage; //!< @brief network capacity usage rate received + float tx_usage; //!< @brief network capacity usage rate transmitted + unsigned int rx_bytes; //!< @brief total bytes received + unsigned int rx_errors; //!< @brief bad packets received + unsigned int tx_bytes; //!< @brief total bytes transmitted + unsigned int tx_errors; //!< @brief packet transmit problems + unsigned int collisions; //!< @brief number of collisions during packet transmissions + + NetworkInfo() + : mtu_errno(0), + ethtool_errno(0), + is_running(false), + interface_name(""), + speed(0.0), + mtu(0), + rx_traffic(0.0), + tx_traffic(0.0), + rx_usage(0.0), + tx_usage(0.0), + rx_bytes(0), + rx_errors(0), + tx_bytes(0), + tx_errors(0), + collisions(0) + { + } + }; + + /** + * @brief determine if it is a supported network + * @param [in] net_info network infomation + * @param [in] index index of network infomation index + * @param [out] stat diagnostic message passed directly to diagnostic publish calls + * @param [out] error_str error string + * @return result of determining whether it is a supported network + */ + bool isSupportedNetwork( + const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, + std::string & error_str); + diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics + rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get Network information char hostname_[HOST_NAME_MAX + 1]; //!< @brief host name std::map bytes_; //!< @brief list of bytes rclcpp::Time last_update_time_; //!< @brief last update time std::vector device_params_; //!< @brief list of devices - NL80211 nl80211_; // !< @brief 802.11 netlink-based interface + NL80211 nl80211_; //!< @brief 802.11 netlink-based interface + int getifaddrs_errno_; //!< @brief errno set by getifaddrs() + std::vector net_info_list_; //!< @brief list of Network information - std::string monitor_program_; //!< @brief nethogs monitor program name - bool nethogs_all_; //!< @brief nethogs result all mode - int traffic_reader_port_; //!< @brief port number to connect to traffic_reader + /** + * @brief CRC errors information + */ + typedef struct crc_errors + { + std::deque errors_queue; //!< @brief queue that holds count of CRC errors + unsigned int last_rx_crc_errors; //!< @brief rx_crc_error at the time of the last monitoring + + crc_errors() : last_rx_crc_errors(0) {} + } crc_errors; + std::map crc_errors_; //!< @brief list of CRC errors + + std::string monitor_program_; //!< @brief nethogs monitor program name + bool nethogs_all_; //!< @brief nethogs result all mode + int traffic_reader_port_; //!< @brief port number to connect to traffic_reader + unsigned int crc_error_check_duration_; //!< @brief CRC error check duration + unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold /** * @brief Network usage status messages diff --git a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp index c319b8e35d5dd..f4ee2de666c22 100644 --- a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp +++ b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp @@ -57,10 +57,12 @@ class NTPMonitor : public rclcpp::Node * @brief function to execute chronyc * @param [out] outOffset offset value of NTP time * @param [out] out_tracking_map "chronyc tracking" output for diagnostic - * @return if error occurred, return error string + * @param [out] pipe2_err_str if pipe2 error occurred, return error string + * @return if chronyc error occurred, return error string */ std::string executeChronyc( - float & outOffset, std::map & out_tracking_map); + float & outOffset, std::map & out_tracking_map, + std::string & pipe2_err_str); diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index 802d905d6bfd5..e114d58770883 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -122,7 +122,8 @@ class ProcessMonitor : public rclcpp::Node memory_tasks_; //!< @brief list of diagnostics tasks for high memory procs rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to execute top command std::string top_output_; //!< @brief output from top command - bool is_top_error_; //!< @brief flag if an error occurs + bool is_top_error_; //!< @brief flag if an top error occurs + bool is_pipe2_error_; //!< @brief flag if an pipe2 error occurs double elapsed_ms_; //!< @brief Execution time of top command std::mutex mutex_; //!< @brief mutex for output from top command rclcpp::CallbackGroup::SharedPtr timer_callback_group_; //!< @brief Callback Group diff --git a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp index 90766fbcc79fc..5e8ad3ff357e9 100644 --- a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp +++ b/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp @@ -134,8 +134,31 @@ void CPUMonitorBase::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & st } // Get CPU Usage - bp::ipstream is_out; - bp::ipstream is_err; + + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + cpu_usage.all.status = CpuStatus::STALE; + publishCpuUsage(cpu_usage); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + cpu_usage.all.status = CpuStatus::STALE; + publishCpuUsage(cpu_usage); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c("mpstat -P ALL 1 1 -o JSON", bp::std_out > is_out, bp::std_err > is_err); c.wait(); diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index d1b15f1231987..3e05e76054f62 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -198,8 +198,31 @@ void HDDMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) for (auto itr = hdd_params_.begin(); itr != hdd_params_.end(); ++itr, ++hdd_index) { // Get summary of disk space usage of ext4 - bp::ipstream is_out; - bp::ipstream is_err; + + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + error_str = "pipe2 error"; + stat.add(fmt::format("HDD {}: status", hdd_index), "pipe2 error"); + stat.add(fmt::format("HDD {}: name", hdd_index), itr->first.c_str()); + stat.add(fmt::format("HDD {}: pipe2", hdd_index), strerror(errno)); + continue; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + error_str = "pipe2 error"; + stat.add(fmt::format("HDD {}: status", hdd_index), "pipe2 error"); + stat.add(fmt::format("HDD {}: name", hdd_index), itr->first.c_str()); + stat.add(fmt::format("HDD {}: pipe2", hdd_index), strerror(errno)); + continue; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + // Invoke shell to use shell wildcard expansion bp::child c( "/bin/sh", "-c", fmt::format("df -Pm {}*", itr->first.c_str()), bp::std_out > is_out, @@ -324,8 +347,24 @@ void HDDMonitor::getHDDParams() std::string HDDMonitor::getDeviceFromMountPoint(const std::string & mount_point) { std::string ret; - bp::ipstream is_out; - bp::ipstream is_err; + + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + RCLCPP_ERROR(get_logger(), "Failed to execute pipe2. %s", strerror(errno)); + return ""; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + RCLCPP_ERROR(get_logger(), "Failed to execute pipe2. %s", strerror(errno)); + return ""; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; bp::child c( "/bin/sh", "-c", fmt::format("findmnt -n -o SOURCE {}", mount_point.c_str()), diff --git a/system/system_monitor/src/mem_monitor/mem_monitor.cpp b/system/system_monitor/src/mem_monitor/mem_monitor.cpp index b6f01b8396aab..489a4dc72bbe8 100644 --- a/system/system_monitor/src/mem_monitor/mem_monitor.cpp +++ b/system/system_monitor/src/mem_monitor/mem_monitor.cpp @@ -48,8 +48,27 @@ void MemMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) const auto t_start = SystemMonitorUtility::startMeasurement(); // Get total amount of free and used memory - bp::ipstream is_out; - bp::ipstream is_err; + + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c("free -tb", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index c1c06ee6f1d4d..983af5668a082 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -47,8 +47,12 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) device_params_( declare_parameter>("devices", std::vector())), monitor_program_(declare_parameter("monitor_program", "greengrass")), - traffic_reader_port_(declare_parameter("traffic_reader_port", TRAFFIC_READER_PORT)) + traffic_reader_port_(declare_parameter("traffic_reader_port", TRAFFIC_READER_PORT)), + crc_error_check_duration_(declare_parameter("crc_error_check_duration", 1)), + crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)) { + using namespace std::literals::chrono_literals; + if (monitor_program_.empty()) { monitor_program_ = GET_ALL_STR; nethogs_all_ = true; @@ -60,23 +64,27 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) updater_.setHardwareID(hostname_); updater_.add("Network Usage", this, &NetMonitor::checkUsage); updater_.add("Network Traffic", this, &NetMonitor::monitorTraffic); + updater_.add("Network CRC Error", this, &NetMonitor::checkCrcError); nl80211_.init(); + + // get Network information for the first time + updateNetworkInfoList(); + + timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::onTimer, this)); } NetMonitor::~NetMonitor() { shutdown_nl80211(); } -void NetMonitor::update() { updater_.force_update(); } - void NetMonitor::shutdown_nl80211() { nl80211_.shutdown(); } -void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::onTimer() { updateNetworkInfoList(); } + +void NetMonitor::updateNetworkInfoList() { - // Remember start time to measure elapsed time - const auto t_start = SystemMonitorUtility::startMeasurement(); + net_info_list_.clear(); if (device_params_.empty()) { - stat.summary(DiagStatus::ERROR, "invalid device parameter"); return; } @@ -86,22 +94,12 @@ void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) rclcpp::Duration duration = this->now() - last_update_time_; // Get network interfaces + getifaddrs_errno_ = 0; if (getifaddrs(&ifas) < 0) { - stat.summary(DiagStatus::ERROR, "getifaddrs error"); - stat.add("getifaddrs", strerror(errno)); + getifaddrs_errno_ = errno; return; } - int level = DiagStatus::OK; - int whole_level = DiagStatus::OK; - int index = 0; - std::string error_str; - float rx_traffic{0.0}; - float tx_traffic{0.0}; - float rx_usage{0.0}; - float tx_usage{0.0}; - std::vector interface_names; - for (ifa = ifas; ifa; ifa = ifa->ifa_next) { // Skip no addr if (!ifa->ifa_addr) { @@ -127,90 +125,117 @@ void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) struct ifreq ifrc; struct ethtool_cmd edata; + net_info_list_.emplace_back(); + auto & net_info = net_info_list_.back(); + + net_info.interface_name = std::string(ifa->ifa_name); + // Get MTU information fd = socket(AF_INET, SOCK_DGRAM, 0); strncpy(ifrm.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); if (ioctl(fd, SIOCGIFMTU, &ifrm) < 0) { - if (errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), ifa->ifa_name); - stat.add("ioctl(SIOCGIFMTU)", strerror(errno)); - - ++index; + net_info.mtu_errno = errno; close(fd); - interface_names.push_back(ifa->ifa_name); continue; } // Get network capacity - float speed = 0.0; strncpy(ifrc.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); ifrc.ifr_data = (caddr_t)&edata; edata.cmd = ETHTOOL_GSET; if (ioctl(fd, SIOCETHTOOL, &ifrc) < 0) { // possibly wireless connection, get bitrate(MBit/s) - speed = nl80211_.getBitrate(ifa->ifa_name); - if (speed <= 0) { - if (errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), ifa->ifa_name); - stat.add("ioctl(SIOCETHTOOL)", strerror(errno)); - - ++index; + net_info.speed = nl80211_.getBitrate(ifa->ifa_name); + if (net_info.speed <= 0) { + net_info.ethtool_errno = errno; close(fd); - interface_names.push_back(ifa->ifa_name); continue; } } else { - speed = edata.speed; + net_info.speed = edata.speed; } - level = (ifa->ifa_flags & IFF_RUNNING) ? DiagStatus::OK : DiagStatus::ERROR; + net_info.is_running = (ifa->ifa_flags & IFF_RUNNING); auto * stats = (struct rtnl_link_stats *)ifa->ifa_data; - if (bytes_.find(ifa->ifa_name) != bytes_.end()) { - rx_traffic = toMbit(stats->rx_bytes - bytes_[ifa->ifa_name].rx_bytes) / duration.seconds(); - tx_traffic = toMbit(stats->tx_bytes - bytes_[ifa->ifa_name].tx_bytes) / duration.seconds(); - rx_usage = rx_traffic / speed; - tx_usage = tx_traffic / speed; + if (bytes_.find(net_info.interface_name) != bytes_.end()) { + net_info.rx_traffic = + toMbit(stats->rx_bytes - bytes_[net_info.interface_name].rx_bytes) / duration.seconds(); + net_info.tx_traffic = + toMbit(stats->tx_bytes - bytes_[net_info.interface_name].tx_bytes) / duration.seconds(); + net_info.rx_usage = net_info.rx_traffic / net_info.speed; + net_info.tx_usage = net_info.tx_traffic / net_info.speed; } - stat.add(fmt::format("Network {}: status", index), usage_dict_.at(level)); - stat.add(fmt::format("Network {}: interface name", index), ifa->ifa_name); - stat.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", rx_usage * 1e+2); - stat.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", tx_usage * 1e+2); - stat.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", rx_traffic); - stat.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", tx_traffic); - stat.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", speed); - stat.add(fmt::format("Network {}: mtu", index), ifrm.ifr_mtu); - stat.add(fmt::format("Network {}: rx_bytes", index), stats->rx_bytes); - stat.add(fmt::format("Network {}: rx_errors", index), stats->rx_errors); - stat.add(fmt::format("Network {}: tx_bytes", index), stats->tx_bytes); - stat.add(fmt::format("Network {}: tx_errors", index), stats->tx_errors); - stat.add(fmt::format("Network {}: collisions", index), stats->collisions); + net_info.mtu = ifrm.ifr_mtu; + net_info.rx_bytes = stats->rx_bytes; + net_info.rx_errors = stats->rx_errors; + net_info.tx_bytes = stats->tx_bytes; + net_info.tx_errors = stats->tx_errors; + net_info.collisions = stats->collisions; close(fd); - bytes_[ifa->ifa_name].rx_bytes = stats->rx_bytes; - bytes_[ifa->ifa_name].tx_bytes = stats->tx_bytes; - - ++index; + bytes_[net_info.interface_name].rx_bytes = stats->rx_bytes; + bytes_[net_info.interface_name].tx_bytes = stats->tx_bytes; - interface_names.push_back(ifa->ifa_name); + // Get the count of CRC errors + crc_errors & crc_ers = crc_errors_[net_info.interface_name]; + crc_ers.errors_queue.push_back(stats->rx_crc_errors - crc_ers.last_rx_crc_errors); + while (crc_ers.errors_queue.size() > crc_error_check_duration_) { + crc_ers.errors_queue.pop_front(); + } + crc_ers.last_rx_crc_errors = stats->rx_crc_errors; } freeifaddrs(ifas); + last_update_time_ = this->now(); +} + +void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); + + if (!checkGeneralInfo(stat)) { + return; + } + + int level = DiagStatus::OK; + int whole_level = DiagStatus::OK; + int index = 0; + std::string error_str; + std::vector interface_names; + + for (const auto & net_info : net_info_list_) { + if (!isSupportedNetwork(net_info, index, stat, error_str)) { + ++index; + interface_names.push_back(net_info.interface_name); + continue; + } + + level = net_info.is_running ? DiagStatus::OK : DiagStatus::ERROR; + + stat.add(fmt::format("Network {}: status", index), usage_dict_.at(level)); + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", net_info.rx_usage * 1e+2); + stat.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", net_info.tx_usage * 1e+2); + stat.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", net_info.rx_traffic); + stat.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", net_info.tx_traffic); + stat.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", net_info.speed); + stat.add(fmt::format("Network {}: mtu", index), net_info.mtu); + stat.add(fmt::format("Network {}: rx_bytes", index), net_info.rx_bytes); + stat.add(fmt::format("Network {}: rx_errors", index), net_info.rx_errors); + stat.add(fmt::format("Network {}: tx_bytes", index), net_info.tx_bytes); + stat.add(fmt::format("Network {}: tx_errors", index), net_info.tx_errors); + stat.add(fmt::format("Network {}: collisions", index), net_info.collisions); + + ++index; + + interface_names.push_back(net_info.interface_name); + } + // Check if specified device exists for (const auto & device : device_params_) { // Skip if all devices specified @@ -234,12 +259,107 @@ void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) stat.summary(whole_level, usage_dict_.at(whole_level)); } - last_update_time_ = this->now(); + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, stat); +} + +void NetMonitor::checkCrcError(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); + + if (!checkGeneralInfo(stat)) { + return; + } + + int whole_level = DiagStatus::OK; + int index = 0; + std::string error_str; + + for (const auto & net_info : net_info_list_) { + if (!isSupportedNetwork(net_info, index, stat, error_str)) { + ++index; + continue; + } + + crc_errors & crc_ers = crc_errors_[net_info.interface_name]; + unsigned int unit_rx_crc_errors = 0; + + for (auto errors : crc_ers.errors_queue) { + unit_rx_crc_errors += errors; + } + + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.add(fmt::format("Network {}: total rx_crc_errors", index), crc_ers.last_rx_crc_errors); + stat.add(fmt::format("Network {}: rx_crc_errors per unit time", index), unit_rx_crc_errors); + + if (unit_rx_crc_errors >= crc_error_count_threshold_) { + whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); + error_str = "CRC error"; + } + + ++index; + } + + if (!error_str.empty()) { + stat.summary(whole_level, error_str); + } else { + stat.summary(whole_level, "OK"); + } // Measure elapsed time since start time and report SystemMonitorUtility::stopMeasurement(t_start, stat); } +bool NetMonitor::checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (device_params_.empty()) { + stat.summary(DiagStatus::ERROR, "invalid device parameter"); + return false; + } + + if (getifaddrs_errno_ != 0) { + stat.summary(DiagStatus::ERROR, "getifaddrs error"); + stat.add("getifaddrs", strerror(getifaddrs_errno_)); + return false; + } + return true; +} + +bool NetMonitor::isSupportedNetwork( + const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, + std::string & error_str) +{ + // MTU information + if (net_info.mtu_errno != 0) { + if (net_info.mtu_errno == ENOTSUP) { + stat.add(fmt::format("Network {}: status", index), "Not Supported"); + } else { + stat.add(fmt::format("Network {}: status", index), "Error"); + error_str = "ioctl error"; + } + + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.add("ioctl(SIOCGIFMTU)", strerror(net_info.mtu_errno)); + return false; + } + + // network capacity + if (net_info.speed <= 0) { + if (net_info.ethtool_errno == ENOTSUP) { + stat.add(fmt::format("Network {}: status", index), "Not Supported"); + } else { + stat.add(fmt::format("Network {}: status", index), "Error"); + error_str = "ioctl error"; + } + + stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); + stat.add("ioctl(SIOCETHTOOL)", strerror(net_info.ethtool_errno)); + return false; + } + return true; +} + #include // workaround for build errors void NetMonitor::monitorTraffic(diagnostic_updater::DiagnosticStatusWrapper & stat) diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 49f5ec57eda4c..10dba2f96e164 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -64,9 +64,15 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) } std::string error_str; + std::string pipe2_err_str; float offset = 0.0f; std::map tracking_map; - error_str = executeChronyc(offset, tracking_map); + error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + if (!pipe2_err_str.empty()) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", pipe2_err_str); + return; + } if (!error_str.empty()) { stat.summary(DiagStatus::ERROR, "chronyc error"); stat.add("chronyc", error_str); @@ -92,12 +98,23 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) } std::string NTPMonitor::executeChronyc( - float & out_offset, std::map & out_tracking_map) + float & out_offset, std::map & out_tracking_map, + std::string & pipe2_err_str) { std::string result; // Tracking chrony status - bp::ipstream is_out; + + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + pipe2_err_str = std::string(strerror(errno)); + return result; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + bp::child c("chronyc tracking", bp::std_out > is_out); c.wait(); if (c.exit_code() != 0) { diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 57e1aaf20f571..f78fc00c430dc 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -34,7 +34,8 @@ ProcessMonitor::ProcessMonitor(const rclcpp::NodeOptions & options) : Node("process_monitor", options), updater_(this), num_of_procs_(declare_parameter("num_of_procs", 5)), - is_top_error_(false) + is_top_error_(false), + is_pipe2_error_(false) { using namespace std::literals::chrono_literals; @@ -67,14 +68,23 @@ void ProcessMonitor::monitorProcesses(diagnostic_updater::DiagnosticStatusWrappe // thread-safe read std::string str; bool is_top_error; + bool is_pipe2_error; double elapsed_ms; { std::lock_guard lock(mutex_); str = top_output_; is_top_error = is_top_error_; + is_pipe2_error = is_pipe2_error_; elapsed_ms = elapsed_ms_; } + if (is_pipe2_error) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", str); + setErrorContent(&load_tasks_, "pipe2 error", "pipe2", str); + setErrorContent(&memory_tasks_, "pipe2 error", "pipe2", str); + return; + } if (is_top_error) { stat.summary(DiagStatus::ERROR, "top error"); stat.add("top", str); @@ -107,13 +117,38 @@ void ProcessMonitor::monitorProcesses(diagnostic_updater::DiagnosticStatusWrappe void ProcessMonitor::getTasksSummary( diagnostic_updater::DiagnosticStatusWrapper & stat, const std::string & output) { - bp::pipe p; + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int p_fd[2]; + if (pipe2(p_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe p{p_fd[0], p_fd[1]}; + std::string line; // Echo output for grep { - bp::ipstream is_out; - bp::ipstream is_err; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c(fmt::format("echo {}", output), bp::std_out > p, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { @@ -126,7 +161,15 @@ void ProcessMonitor::getTasksSummary( } // Find matching pattern of summary { - bp::ipstream is_out; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + bp::child c("grep Tasks:", bp::std_out > is_out, bp::std_in < p); c.wait(); // no matching line @@ -158,13 +201,37 @@ void ProcessMonitor::getTasksSummary( void ProcessMonitor::removeHeader( diagnostic_updater::DiagnosticStatusWrapper & stat, std::string & output) { - bp::pipe p1; - bp::pipe p2; + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int p1_fd[2]; + if (pipe2(p1_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe p1{p1_fd[0], p1_fd[1]}; + + int p2_fd[2]; + if (pipe2(p2_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe p2{p2_fd[0], p2_fd[1]}; + std::ostringstream os; // Echo output for sed { - bp::ipstream is_err; + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c(fmt::format("echo {}", output), bp::std_out > p1, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { @@ -176,7 +243,15 @@ void ProcessMonitor::removeHeader( } // Remove %Cpu section { - bp::ipstream is_err; + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c("sed \"/^%Cpu/d\"", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); c.wait(); // no matching line @@ -188,8 +263,24 @@ void ProcessMonitor::removeHeader( } // Remove header { - bp::ipstream is_out; - bp::ipstream is_err; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + stat.summary(DiagStatus::ERROR, "pipe2 error"); + stat.add("pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c("sed \"1,6d\"", bp::std_out > is_out, bp::std_err > is_err, bp::std_in < p2); c.wait(); // no matching line @@ -206,12 +297,34 @@ void ProcessMonitor::removeHeader( void ProcessMonitor::getHighLoadProcesses(const std::string & output) { - bp::pipe p; + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int p_fd[2]; + if (pipe2(p_fd, O_CLOEXEC) != 0) { + setErrorContent(&load_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe p{p_fd[0], p_fd[1]}; + std::ostringstream os; // Echo output for sed - bp::ipstream is_out; - bp::ipstream is_err; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + setErrorContent(&load_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + setErrorContent(&load_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c(fmt::format("echo {}", output), bp::std_out > p, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { @@ -226,14 +339,42 @@ void ProcessMonitor::getHighLoadProcesses(const std::string & output) void ProcessMonitor::getHighMemoryProcesses(const std::string & output) { - bp::pipe p1; - bp::pipe p2; + // boost::process create file descriptor without O_CLOEXEC required for multithreading. + // So create file descriptor with O_CLOEXEC and pass it to boost::process. + int p1_fd[2]; + if (pipe2(p1_fd, O_CLOEXEC) != 0) { + setErrorContent(&memory_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe p1{p1_fd[0], p1_fd[1]}; + + int p2_fd[2]; + if (pipe2(p2_fd, O_CLOEXEC) != 0) { + setErrorContent(&memory_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe p2{p2_fd[0], p2_fd[1]}; + std::ostringstream os; // Echo output for sed { - bp::ipstream is_out; - bp::ipstream is_err; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + setErrorContent(&memory_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + setErrorContent(&memory_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + bp::child c(fmt::format("echo {}", output), bp::std_out > p1, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { @@ -244,8 +385,22 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) } // Sort by memory usage { - bp::ipstream is_out; - bp::ipstream is_err; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + setErrorContent(&memory_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + setErrorContent(&memory_tasks_, "pipe2 error", "pipe2", strerror(errno)); + return; + } + 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); c.wait(); if (c.exit_code() != 0) { @@ -266,8 +421,22 @@ void ProcessMonitor::getTopratedProcesses( return; } - bp::ipstream is_out; - bp::ipstream is_err; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + setErrorContent(tasks, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + setErrorContent(tasks, "pipe2 error", "pipe2", strerror(errno)); + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + std::ostringstream os; bp::child c( @@ -329,8 +498,26 @@ void ProcessMonitor::onTimer() tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("execution_time"); - bp::ipstream is_err; - bp::ipstream is_out; + int out_fd[2]; + if (pipe2(out_fd, O_CLOEXEC) != 0) { + std::lock_guard lock(mutex_); + top_output_ = std::string(strerror(errno)); + is_pipe2_error_ = true; + return; + } + bp::pipe out_pipe{out_fd[0], out_fd[1]}; + bp::ipstream is_out{std::move(out_pipe)}; + + int err_fd[2]; + if (pipe2(err_fd, O_CLOEXEC) != 0) { + std::lock_guard lock(mutex_); + top_output_ = std::string(strerror(errno)); + is_pipe2_error_ = true; + return; + } + bp::pipe err_pipe{err_fd[0], err_fd[1]}; + bp::ipstream is_err{std::move(err_pipe)}; + std::ostringstream os; // Get processes @@ -351,6 +538,7 @@ void ProcessMonitor::onTimer() std::lock_guard lock(mutex_); top_output_ = os.str(); is_top_error_ = is_top_error; + is_pipe2_error_ = false; elapsed_ms_ = elapsed_ms; } } diff --git a/system/velodyne_monitor/Readme.md b/system/velodyne_monitor/Readme.md index 76b5901a9b47f..3e508375fd801 100644 --- a/system/velodyne_monitor/Readme.md +++ b/system/velodyne_monitor/Readme.md @@ -61,7 +61,8 @@ None ### Config files Config files for several velodyne models are prepared. -The `temp_**` parameters are set with reference to the operational temperature from each datasheet. +The `temp_***` parameters are set with reference to the operational temperature from each datasheet. +Moreover, the `temp_hot_***` of each model are set highly as 20 from operational temperature. Now, `VLP-16.param.yaml` is used as default argument because it is lowest spec. | Model Name | Config name | Operational Temperature [℃] | diff --git a/system/velodyne_monitor/config/HDL-32E.param.yaml b/system/velodyne_monitor/config/HDL-32E.param.yaml index e5b53b7a41f7b..ce513578893a5 100644 --- a/system/velodyne_monitor/config/HDL-32E.param.yaml +++ b/system/velodyne_monitor/config/HDL-32E.param.yaml @@ -4,7 +4,7 @@ timeout: 5.0 temp_cold_warn: -5.0 temp_cold_error: -10.0 - temp_hot_warn: 55.0 - temp_hot_error: 60.0 + temp_hot_warn: 75.0 + temp_hot_error: 80.0 rpm_ratio_warn: 0.80 rpm_ratio_error: 0.70 diff --git a/system/velodyne_monitor/config/VLP-16.param.yaml b/system/velodyne_monitor/config/VLP-16.param.yaml index e5b53b7a41f7b..ce513578893a5 100644 --- a/system/velodyne_monitor/config/VLP-16.param.yaml +++ b/system/velodyne_monitor/config/VLP-16.param.yaml @@ -4,7 +4,7 @@ timeout: 5.0 temp_cold_warn: -5.0 temp_cold_error: -10.0 - temp_hot_warn: 55.0 - temp_hot_error: 60.0 + temp_hot_warn: 75.0 + temp_hot_error: 80.0 rpm_ratio_warn: 0.80 rpm_ratio_error: 0.70 diff --git a/system/velodyne_monitor/config/VLP-32C.param.yaml b/system/velodyne_monitor/config/VLP-32C.param.yaml index 8fb38d4e096b2..56b5dd942f69e 100644 --- a/system/velodyne_monitor/config/VLP-32C.param.yaml +++ b/system/velodyne_monitor/config/VLP-32C.param.yaml @@ -4,7 +4,7 @@ timeout: 5.0 temp_cold_warn: -15.0 temp_cold_error: -20.0 - temp_hot_warn: 55.0 - temp_hot_error: 60.0 + temp_hot_warn: 75.0 + temp_hot_error: 80.0 rpm_ratio_warn: 0.80 rpm_ratio_error: 0.70 diff --git a/system/velodyne_monitor/config/VLS-128.param.yaml b/system/velodyne_monitor/config/VLS-128.param.yaml index 8fb38d4e096b2..56b5dd942f69e 100644 --- a/system/velodyne_monitor/config/VLS-128.param.yaml +++ b/system/velodyne_monitor/config/VLS-128.param.yaml @@ -4,7 +4,7 @@ timeout: 5.0 temp_cold_warn: -15.0 temp_cold_error: -20.0 - temp_hot_warn: 55.0 - temp_hot_error: 60.0 + temp_hot_warn: 75.0 + temp_hot_error: 80.0 rpm_ratio_warn: 0.80 rpm_ratio_error: 0.70 diff --git a/system/velodyne_monitor/config/Velarray_M1600.param.yaml b/system/velodyne_monitor/config/Velarray_M1600.param.yaml index 35d3213a5c66c..ac275a2c188a7 100644 --- a/system/velodyne_monitor/config/Velarray_M1600.param.yaml +++ b/system/velodyne_monitor/config/Velarray_M1600.param.yaml @@ -4,7 +4,7 @@ timeout: 5.0 temp_cold_warn: -35.0 temp_cold_error: -40.0 - temp_hot_warn: 80.0 - temp_hot_error: 85.0 + temp_hot_warn: 100.0 + temp_hot_error: 105.0 rpm_ratio_warn: 0.80 rpm_ratio_error: 0.70 diff --git a/vehicle/external_cmd_converter/package.xml b/vehicle/external_cmd_converter/package.xml index 0e8962f5b7bf8..af7200c6f51bd 100644 --- a/vehicle/external_cmd_converter/package.xml +++ b/vehicle/external_cmd_converter/package.xml @@ -4,10 +4,10 @@ external_cmd_converter 0.1.0 The external_cmd_converter package - Horibe Takamasa + Takamasa Horibe Apache License 2.0 - Horibe Takamasa + Takamasa Horibe ament_cmake_auto diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml index 4f36c95e5d82d..cf1e55978bfb1 100644 --- a/vehicle/raw_vehicle_cmd_converter/package.xml +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -4,11 +4,11 @@ raw_vehicle_cmd_converter 0.1.0 The raw_vehicle_cmd_converter package - Horibe Takamasa + Takamasa Horibe Apache License 2.0 - Makoto Kurihara - Horibe Takamasa + Makoto Kurihara + Takamasa Horibe ament_cmake_auto