diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index e4055a49f688e..614496c01d6cc 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -26,6 +26,8 @@ - source: .github/workflows/build-and-test-differential-self-hosted.yaml - source: .github/workflows/build-and-test-self-hosted.yaml - source: .github/workflows/check-build-depends.yaml + - source: .github/workflows/clang-tidy-pr-comments.yaml + - source: .github/workflows/sync-files.yaml - repository: autowarefoundation/autoware-documentation files: diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml index 824ac368f936c..a15ced5f3b0c8 100644 --- a/.github/workflows/build-and-test-differential-self-hosted.yaml +++ b/.github/workflows/build-and-test-differential-self-hosted.yaml @@ -10,7 +10,7 @@ on: jobs: prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@tier4/proposal + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: label: ARM64 @@ -21,20 +21,20 @@ jobs: container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get modified packages id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - name: Build if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} @@ -42,7 +42,7 @@ jobs: - name: Test if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 6e8ebef574ed5..7e3d186886b6b 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -12,20 +12,20 @@ jobs: uses: styfle/cancel-workflow-action@0.9.1 - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get modified packages id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - name: Build if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} @@ -34,7 +34,7 @@ jobs: - name: Test id: test if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} @@ -54,22 +54,22 @@ jobs: needs: build-and-test-differential steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get modified packages id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - name: Run clang-tidy if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@tier4/proposal + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/tier4/proposal/.clang-tidy + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml index c5a5ac4734bc5..9f31a07199783 100644 --- a/.github/workflows/build-and-test-self-hosted.yaml +++ b/.github/workflows/build-and-test-self-hosted.yaml @@ -2,7 +2,7 @@ name: build-and-test-self-hosted on: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: @@ -11,18 +11,18 @@ jobs: container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get self packages id: get-self-packages - uses: autowarefoundation/autoware-github-actions/get-self-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} @@ -30,7 +30,7 @@ jobs: - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 1e422cc380f11..125d209ac1603 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -3,7 +3,7 @@ name: build-and-test on: push: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: @@ -13,18 +13,18 @@ jobs: container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get self packages id: get-self-packages - uses: autowarefoundation/autoware-github-actions/get-self-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} @@ -33,7 +33,7 @@ jobs: - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} id: test - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index d8f731787f4f0..fbea900cde9a7 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -14,17 +14,17 @@ jobs: uses: styfle/cancel-workflow-action@0.9.1 - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get self packages id: get-self-packages - uses: autowarefoundation/autoware-github-actions/get-self-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - name: Build - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index ea035a25f3000..dc53df49ac20c 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Download analysis results run: | @@ -29,7 +29,7 @@ jobs: echo ::set-output name=pr-head-ref::"$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" - name: Check out PR head - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index bb11e4dc75e78..b7b009fb00263 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -2,7 +2,7 @@ name: delete-closed-pr-docs on: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: @@ -10,11 +10,11 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 - name: Delete closed PR docs - uses: autowarefoundation/autoware-github-actions/delete-closed-pr-docs@tier4/proposal + uses: autowarefoundation/autoware-github-actions/delete-closed-pr-docs@v1 with: token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index ce35cb8f76ce5..7f0f72d94a799 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -4,7 +4,6 @@ on: push: branches: - main - - tier4/proposal paths: - mkdocs.yaml - "**/*.md" @@ -20,7 +19,7 @@ on: jobs: prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@tier4/proposal + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: label: documentation @@ -30,12 +29,12 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 ref: ${{ github.event.pull_request.head.sha }} - name: Deploy docs - uses: autowarefoundation/autoware-github-actions/deploy-docs@tier4/proposal + uses: autowarefoundation/autoware-github-actions/deploy-docs@v1 with: token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index 50b6778a25c0a..93e05dc2c79e5 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -8,9 +8,9 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Run pre-commit - uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal + uses: autowarefoundation/autoware-github-actions/pre-commit@v1 with: pre-commit-config: .pre-commit-config-optional.yaml diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 9eb501a19dac9..e1b72f706881c 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -8,9 +8,9 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Run pre-commit - uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal + uses: autowarefoundation/autoware-github-actions/pre-commit@v1 with: pre-commit-config: .pre-commit-config.yaml diff --git a/.github/workflows/semantic-pull-request.yaml b/.github/workflows/semantic-pull-request.yaml index 0edf1946330d3..71224c224ec0f 100644 --- a/.github/workflows/semantic-pull-request.yaml +++ b/.github/workflows/semantic-pull-request.yaml @@ -9,4 +9,4 @@ on: jobs: semantic-pull-request: - uses: autowarefoundation/autoware-github-actions/.github/workflows/semantic-pull-request.yaml@tier4/proposal + uses: autowarefoundation/autoware-github-actions/.github/workflows/semantic-pull-request.yaml@v1 diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index 1b5c52cb40ec4..eb18ccdba38d0 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -8,9 +8,9 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@tier4/proposal + uses: autowarefoundation/autoware-github-actions/spell-check@v1 with: cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 0c5b4b04fa084..32f4613f6de7a 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -2,7 +2,7 @@ name: sync-files on: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: @@ -17,6 +17,6 @@ jobs: private_key: ${{ secrets.PRIVATE_KEY }} - name: Run sync-files - uses: autowarefoundation/autoware-github-actions/sync-files@tier4/proposal + uses: autowarefoundation/autoware-github-actions/sync-files@v1 with: token: ${{ steps.generate-token.outputs.token }} diff --git a/CPPLINT.cfg b/CPPLINT.cfg index 4dbbe0596b5db..1e2521f0b6442 100644 --- a/CPPLINT.cfg +++ b/CPPLINT.cfg @@ -9,5 +9,5 @@ filter=-whitespace/braces # we wrap open curly braces for namespaces, cl filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space filter=-whitespace/parens # we allow closing parenthesis to be on the next line filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon -filter=-build/header_guard # TODO(Kenji Miyake): Support ROS-style rule in cpplint or add auto-fix script in pre-commit +filter=-build/header_guard # we automatically fix the names of header guards using pre-commit filter=-build/include_order # we use the custom include order diff --git a/build_depends.repos b/build_depends.repos index f7785c24cfeae..fa60d2107015e 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -7,11 +7,11 @@ repositories: core/common: type: git url: https://github.com/autowarefoundation/autoware_common.git - version: tier4/proposal + version: main core/autoware: type: git url: https://github.com/autowarefoundation/autoware.core.git - version: tier4/proposal + version: main # universe universe/tier4_autoware_msgs: type: git diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml index ae5a39dd99ceb..fd21436b3909b 100644 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml @@ -29,6 +29,12 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 + + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py index 57e172ffda42d..7e8dda8749251 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py @@ -59,7 +59,7 @@ def add_launch_arg(name: str, default_value=None): "max_height": 2.0, "angle_min": -3.141592, # -M_PI "angle_max": 3.141592, # M_PI - "angle_increment": 0.00436332222, # 0.25*M_PI/180.0 + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 "scan_time": 0.0, "range_min": 1.0, "range_max": 200.0, diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 8ceb7496ab828..8525313a03b38 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -24,7 +24,7 @@ - + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 0bec3d13b6a35..920d6e8a80cd0 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -1,12 +1,13 @@ + + - - + + - @@ -17,7 +18,7 @@ - + @@ -25,16 +26,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + - diff --git a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index f696e6b33d73d..b33eac296abc9 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -1,9 +1,9 @@ + - @@ -17,7 +17,7 @@ - + diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index dc8718f76449b..ec50a113af4d9 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -21,9 +21,10 @@ ## Parameters -| Parameter | Type | Description | -| -------------- | ------ | ----------------- | -| `output_frame` | String | output's frame id | +| Parameter | Type | Description | +| --------------------- | ------ | -------------------------------- | +| `output_frame` | String | output's frame id | +| `message_timeout_sec` | Double | delay tolerance time for message | ## Assumptions / Known limits diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index f95374a48a12d..b7dc9ea3414f4 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -54,7 +54,10 @@ class GyroOdometer : public rclcpp::Node tf2_ros::TransformListener tf_listener_; std::string output_frame_; + double message_timeout_sec_; + geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr_; + sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr_; }; #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index b3148aec12e55..9a0a78ad4bd39 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -7,6 +7,7 @@ + @@ -18,5 +19,6 @@ + diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index e4ab35dbace19..7dd1da5c0f0e7 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -24,7 +24,8 @@ GyroOdometer::GyroOdometer() : Node("gyro_odometer"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), - output_frame_(declare_parameter("base_link", "base_link")) + output_frame_(declare_parameter("base_link", "base_link")), + message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)) { vehicle_twist_with_cov_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, @@ -47,25 +48,49 @@ void GyroOdometer::callbackTwistWithCovariance( { // TODO(YamatoAndo) trans from twist_with_cov_msg_ptr->header to base_frame twist_with_cov_msg_ptr_ = twist_with_cov_msg_ptr; + + if (!imu_msg_ptr_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + return; + } + const double imu_dt = std::abs((this->now() - imu_msg_ptr_->header.stamp).seconds()); + if (imu_dt > message_timeout_sec_) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Imu msg is timeout. imu_dt: %lf[sec], tolerance %lf[sec]", imu_dt, message_timeout_sec_); + return; + } } void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { + imu_msg_ptr_ = imu_msg_ptr; + if (!twist_with_cov_msg_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); + return; + } + + const double twist_dt = std::abs((this->now() - twist_with_cov_msg_ptr_->header.stamp).seconds()); + if (twist_dt > message_timeout_sec_) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Twist msg is timeout. twist_dt: %lf[sec], tolerance %lf[sec]", twist_dt, + message_timeout_sec_); return; } geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr = std::make_shared(); - getTransform(output_frame_, imu_msg_ptr->header.frame_id, tf_base2imu_ptr); + getTransform(output_frame_, imu_msg_ptr_->header.frame_id, tf_base2imu_ptr); geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; + angular_velocity.header = imu_msg_ptr_->header; + angular_velocity.vector = imu_msg_ptr_->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; transformed_angular_velocity.header = tf_base2imu_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr); // clear imu yaw bias if vehicle is stopped @@ -77,14 +102,14 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m // TODO(YamatoAndo) move code geometry_msgs::msg::TwistStamped twist; - twist.header.stamp = imu_msg_ptr->header.stamp; + twist.header.stamp = imu_msg_ptr_->header.stamp; twist.header.frame_id = output_frame_; twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; twist.twist.angular.z = transformed_angular_velocity.vector.z; // TODO(YamatoAndo) yaw_rate only twist_pub_->publish(twist); geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance; - twist_with_covariance.header.stamp = imu_msg_ptr->header.stamp; + twist_with_covariance.header.stamp = imu_msg_ptr_->header.stamp; twist_with_covariance.header.frame_id = output_frame_; twist_with_covariance.twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; twist_with_covariance.twist.twist.angular.z = @@ -98,7 +123,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m twist_with_covariance.twist.covariance[14] = 10000.0; twist_with_covariance.twist.covariance[21] = 10000.0; twist_with_covariance.twist.covariance[28] = 10000.0; - twist_with_covariance.twist.covariance[35] = imu_msg_ptr->angular_velocity_covariance[8]; + twist_with_covariance.twist.covariance[35] = imu_msg_ptr_->angular_velocity_covariance[8]; twist_with_covariance_pub_->publish(twist_with_covariance); } diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 9b044ec409934..7b62785f7f11a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -30,6 +30,12 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 + + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index f4407819a0ae0..a74c5b47b51e7 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -113,6 +113,13 @@ class NDTScanMatcher : public rclcpp::Node const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr); + bool validateTimeStampDifference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec); + bool validatePositionDifference( + const geometry_msgs::msg::Point & target_point, + const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_); + void timerDiagnostic(); rclcpp::Subscription::SharedPtr initial_pose_sub_; @@ -154,6 +161,8 @@ class NDTScanMatcher : public rclcpp::Node std::string map_frame_; double converged_param_transform_probability_; int initial_estimate_particles_num_; + double initial_pose_timeout_sec_; + double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; float oscillation_threshold_; std::array output_pose_covariance_; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index c83659f337958..9b6bb71d60426 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -8,6 +8,7 @@ ament_cmake_auto diagnostic_msgs + fmt geometry_msgs nav_msgs ndt diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 855b7c9750623..dca714376599a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -104,6 +104,8 @@ NDTScanMatcher::NDTScanMatcher() map_frame_("map"), converged_param_transform_probability_(4.5), initial_estimate_particles_num_(100), + initial_pose_timeout_sec_(1.0), + initial_pose_distance_tolerance_m_(10.0), inversion_vector_threshold_(-0.9), oscillation_threshold_(10) { @@ -169,6 +171,12 @@ NDTScanMatcher::NDTScanMatcher() initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_); + initial_pose_timeout_sec_ = + this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); + + initial_pose_distance_tolerance_m_ = this->declare_parameter( + "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_); + std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { @@ -433,9 +441,27 @@ void NDTScanMatcher::callbackSensorPoints( initial_pose_msg_ptr_array_, sensor_ros_time, initial_pose_old_msg_ptr, initial_pose_new_msg_ptr); popOldPose(initial_pose_msg_ptr_array_, sensor_ros_time); - // TODO(Tier IV): check pose_timestamp - sensor_ros_time + + // check the time stamp + bool valid_old_timestamp = validateTimeStampDifference( + initial_pose_old_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); + bool valid_new_timestamp = validateTimeStampDifference( + initial_pose_new_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); + + // check the position jumping (ex. immediately after the initial pose estimation) + bool valid_new_to_old_distance = validatePositionDifference( + initial_pose_old_msg_ptr->pose.pose.position, initial_pose_new_msg_ptr->pose.pose.position, + initial_pose_distance_tolerance_m_); + + // must all validations are true + if (!(valid_old_timestamp && valid_new_timestamp && valid_new_to_old_distance)) { + RCLCPP_WARN(get_logger(), "Validation error."); + return; + } + const auto initial_pose_msg = interpolatePose(*initial_pose_old_msg_ptr, *initial_pose_new_msg_ptr, sensor_ros_time); + // enf of critical section for initial_pose_msg_ptr_array_ initial_pose_array_lock.unlock(); @@ -688,3 +714,35 @@ bool NDTScanMatcher::getTransform( } return true; } + +bool NDTScanMatcher::validateTimeStampDifference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) +{ + const double dt = std::abs((target_time - reference_time).seconds()); + if (dt > time_tolerance_sec) { + RCLCPP_WARN( + get_logger(), + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + return false; + } + return true; +} + +bool NDTScanMatcher::validatePositionDifference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) +{ + double distance = norm(target_point, reference_point); + if (distance > distance_tolerance_m_) { + RCLCPP_WARN( + get_logger(), + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + return false; + } + return true; +} diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index 41f3bd75bc722..ea18545da4970 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -123,7 +123,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} ${PCL_LIBRARIES} ) diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index c1c99076a4b47..99b23bf4bc3ac 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -130,7 +130,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND TORCH_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${TORCH_LIBRARIES} ) diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 8c386f1bb2f44..86de43c839ce6 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -21,7 +21,7 @@ ament_auto_add_library(map_based_prediction_node SHARED ) rclcpp_components_register_node(map_based_prediction_node - PLUGIN "MapBasedPredictionROS" + PLUGIN "map_based_prediction::MapBasedPredictionROS" EXECUTABLE map_based_prediction ) diff --git a/perception/map_based_prediction/include/cubic_spline.hpp b/perception/map_based_prediction/include/cubic_spline.hpp index 7e0826ceb1197..d55f8ee9dde43 100644 --- a/perception/map_based_prediction/include/cubic_spline.hpp +++ b/perception/map_based_prediction/include/cubic_spline.hpp @@ -30,6 +30,8 @@ #include #include +namespace map_based_prediction +{ static std::vector vec_diff(const std::vector & input) { std::vector output; @@ -250,5 +252,6 @@ class Spline2D return out_s; } }; +} // namespace map_based_prediction #endif // CUBIC_SPLINE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction.hpp b/perception/map_based_prediction/include/map_based_prediction.hpp index e297f51ae357d..19678f8c2a482 100644 --- a/perception/map_based_prediction/include/map_based_prediction.hpp +++ b/perception/map_based_prediction/include/map_based_prediction.hpp @@ -22,9 +22,20 @@ #include +namespace map_based_prediction +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; + struct DynamicObjectWithLanes { - autoware_auto_perception_msgs::msg::TrackedObject object; + TrackedObject object; std::vector> lanes; std::vector confidence; }; @@ -47,33 +58,29 @@ class MapBasedPrediction bool getPredictedPath( const double height, const double current_d_position, const double current_d_velocity, const double current_s_position, const double current_s_velocity, - const std_msgs::msg::Header & origin_header, Spline2D & spline2d, - autoware_auto_perception_msgs::msg::PredictedPath & path) const; + const std_msgs::msg::Header & origin_header, Spline2D & spline2d, PredictedPath & path) const; void getLinearPredictedPath( const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const; + PredictedPath & predicted_path) const; - void normalizeLikelihood( - autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics); + void normalizeLikelihood(PredictedObjectKinematics & predicted_object_kinematics); - autoware_auto_perception_msgs::msg::PredictedObjectKinematics convertToPredictedKinematics( - const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object); + PredictedObjectKinematics convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object); public: MapBasedPrediction( double interpolating_resolution, double time_horizon, double sampling_delta_time); bool doPrediction( - const DynamicObjectWithLanesArray & in_objects, - std::vector & out_objects); + const DynamicObjectWithLanesArray & in_objects, std::vector & out_objects); bool doLinearPrediction( - const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, - std::vector & out_objects); + const PredictedObjects & in_objects, std::vector & out_objects); - autoware_auto_perception_msgs::msg::PredictedObject convertToPredictedObject( - const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object); + PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); }; +} // namespace map_based_prediction #endif // MAP_BASED_PREDICTION_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction_ros.hpp b/perception/map_based_prediction/include/map_based_prediction_ros.hpp index 5f477f0133f79..693a711cfa05e 100644 --- a/perception/map_based_prediction/include/map_based_prediction_ros.hpp +++ b/perception/map_based_prediction/include/map_based_prediction_ros.hpp @@ -62,6 +62,15 @@ class TrafficRules; } // namespace traffic_rules } // namespace lanelet +namespace map_based_prediction +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +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::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + struct ObjectData { lanelet::ConstLanelets current_lanelets; @@ -95,9 +104,9 @@ class MapBasedPredictionROS : public rclcpp::Node double diff_dist_threshold_to_left_bound_; double diff_dist_threshold_to_right_bound_; - rclcpp::Subscription::SharedPtr sub_objects_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Publisher::SharedPtr pub_objects_; + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_markers_; std::unordered_map> object_buffer_; @@ -113,32 +122,28 @@ class MapBasedPredictionROS : public rclcpp::Node bool getSelfPose(geometry_msgs::msg::Pose & self_pose, const std_msgs::msg::Header & header); bool getSelfPoseInMap(geometry_msgs::msg::Pose & self_pose); - double getObjectYaw(const autoware_auto_perception_msgs::msg::TrackedObject & object); + double getObjectYaw(const TrackedObject & object); double calculateLikelihood( - const std::vector & path, - const autoware_auto_perception_msgs::msg::TrackedObject & object); + const std::vector & path, const TrackedObject & object); void addValidPath( const lanelet::routing::LaneletPaths & candidate_paths, lanelet::routing::LaneletPaths & valid_paths); - void objectsCallback( - const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects); - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); + void mapCallback(const HADMapBin::ConstSharedPtr msg); bool getClosestLanelets( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::ConstLanelets & closest_lanelets); + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::ConstLanelets & closest_lanelets); bool checkCloseLaneletCondition( - const std::pair & lanelet, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std::pair & lanelet, const TrackedObject & object, const lanelet::BasicPoint2d & search_point); void removeInvalidObject(const double current_time); bool updateObjectBuffer( - const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std_msgs::msg::Header & header, const TrackedObject & object, lanelet::ConstLanelets & current_lanelets); void updatePossibleLanelets( const std::string object_id, const lanelet::routing::LaneletPaths & paths); @@ -148,11 +153,12 @@ class MapBasedPredictionROS : public rclcpp::Node double calcLeftLateralOffset( const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); Maneuver detectLaneChange( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::ConstLanelet & current_lanelet, const double current_time); + const TrackedObject & object, const lanelet::ConstLanelet & current_lanelet, + const double current_time); public: explicit MapBasedPredictionROS(const rclcpp::NodeOptions & node_options); }; +} // namespace map_based_prediction #endif // MAP_BASED_PREDICTION_ROS_HPP_ diff --git a/perception/map_based_prediction/src/map_based_prediction.cpp b/perception/map_based_prediction/src/map_based_prediction.cpp index e289bc0e8b6d0..7bd9f7498590d 100644 --- a/perception/map_based_prediction/src/map_based_prediction.cpp +++ b/perception/map_based_prediction/src/map_based_prediction.cpp @@ -22,6 +22,8 @@ #include #include +namespace map_based_prediction +{ MapBasedPrediction::MapBasedPrediction( double interpolating_resolution, double time_horizon, double sampling_delta_time) : interpolating_resolution_(interpolating_resolution), @@ -31,11 +33,10 @@ MapBasedPrediction::MapBasedPrediction( } bool MapBasedPrediction::doPrediction( - const DynamicObjectWithLanesArray & in_objects, - std::vector & out_objects) + const DynamicObjectWithLanesArray & in_objects, std::vector & out_objects) { for (auto & object_with_lanes : in_objects.objects) { - autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + PredictedObject tmp_object; tmp_object = convertToPredictedObject(object_with_lanes.object); for (size_t path_id = 0; path_id < object_with_lanes.lanes.size(); ++path_id) { std::vector tmp_x; @@ -101,7 +102,7 @@ bool MapBasedPrediction::doPrediction( std::fabs(object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.x); // Predict Path - autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + PredictedPath predicted_path; getPredictedPath( object_point.z, current_d_position, current_d_velocity, current_s_position, current_s_velocity, in_objects.header, spline2d, predicted_path); @@ -135,15 +136,14 @@ bool MapBasedPrediction::doPrediction( } bool MapBasedPrediction::doLinearPrediction( - const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, - std::vector & out_objects) + const PredictedObjects & in_objects, std::vector & out_objects) { for (const auto & object : in_objects.objects) { - autoware_auto_perception_msgs::msg::PredictedPath path; + PredictedPath path; getLinearPredictedPath( object.kinematics.initial_pose_with_covariance.pose, object.kinematics.initial_twist_with_covariance.twist, path); - autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + PredictedObject tmp_object; tmp_object = object; tmp_object.kinematics.predicted_paths.push_back(path); out_objects.push_back(tmp_object); @@ -152,10 +152,9 @@ bool MapBasedPrediction::doLinearPrediction( return true; } -autoware_auto_perception_msgs::msg::PredictedObject MapBasedPrediction::convertToPredictedObject( - const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +PredictedObject MapBasedPrediction::convertToPredictedObject(const TrackedObject & tracked_object) { - autoware_auto_perception_msgs::msg::PredictedObject output; + PredictedObject output; output.object_id = tracked_object.object_id; output.existence_probability = tracked_object.existence_probability; output.classification = tracked_object.classification; @@ -164,11 +163,10 @@ autoware_auto_perception_msgs::msg::PredictedObject MapBasedPrediction::convertT return output; } -autoware_auto_perception_msgs::msg::PredictedObjectKinematics -MapBasedPrediction::convertToPredictedKinematics( - const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object) +PredictedObjectKinematics MapBasedPrediction::convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object) { - autoware_auto_perception_msgs::msg::PredictedObjectKinematics output; + PredictedObjectKinematics output; output.initial_pose_with_covariance = tracked_object.pose_with_covariance; output.initial_twist_with_covariance = tracked_object.twist_with_covariance; output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; @@ -176,7 +174,7 @@ MapBasedPrediction::convertToPredictedKinematics( } void MapBasedPrediction::normalizeLikelihood( - autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics) + PredictedObjectKinematics & predicted_object_kinematics) { // might not be the smartest way double sum_confidence = 0.0; @@ -193,7 +191,7 @@ bool MapBasedPrediction::getPredictedPath( const double height, const double current_d_position, const double current_d_velocity, const double current_s_position, const double current_s_velocity, [[maybe_unused]] const std_msgs::msg::Header & origin_header, Spline2D & spline2d, - autoware_auto_perception_msgs::msg::PredictedPath & path) const + PredictedPath & path) const { // Quintic polynomial for d // A = np.array([[T**3, T**4, T**5], @@ -269,7 +267,7 @@ bool MapBasedPrediction::getPredictedPath( void MapBasedPrediction::getLinearPredictedPath( const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + PredictedPath & predicted_path) const { const double & sampling_delta_time = sampling_delta_time_; const double & time_horizon = time_horizon_; @@ -302,3 +300,4 @@ void MapBasedPrediction::getLinearPredictedPath( predicted_path.confidence = 1.0; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_delta_time); } +} // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_ros.cpp b/perception/map_based_prediction/src/map_based_prediction_ros.cpp index 4b77aabb818b6..19df2894e4087 100644 --- a/perception/map_based_prediction/src/map_based_prediction_ros.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_ros.cpp @@ -37,6 +37,8 @@ #include #include +namespace +{ std::string toHexString(const unique_identifier_msgs::msg::UUID & id) { std::stringstream ss; @@ -45,7 +47,9 @@ std::string toHexString(const unique_identifier_msgs::msg::UUID & id) } return ss.str(); } - +} // namespace +namespace map_based_prediction +{ MapBasedPredictionROS::MapBasedPredictionROS(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), interpolating_resolution_(0.5), @@ -79,21 +83,19 @@ MapBasedPredictionROS::MapBasedPredictionROS(const rclcpp::NodeOptions & node_op map_based_prediction_ = std::make_shared( interpolating_resolution_, prediction_time_horizon_, prediction_sampling_delta_time_); - sub_objects_ = this->create_subscription( + sub_objects_ = this->create_subscription( "/perception/object_recognition/tracking/objects", 1, std::bind(&MapBasedPredictionROS::objectsCallback, this, std::placeholders::_1)); - sub_map_ = this->create_subscription( + sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionROS::mapCallback, this, std::placeholders::_1)); - pub_objects_ = this->create_publisher( - "objects", rclcpp::QoS{1}); + pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); pub_markers_ = this->create_publisher( "objects_path_markers", rclcpp::QoS{1}); } -double MapBasedPredictionROS::getObjectYaw( - const autoware_auto_perception_msgs::msg::TrackedObject & object) +double MapBasedPredictionROS::getObjectYaw(const TrackedObject & object) { if (object.kinematics.orientation_availability) { return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -117,8 +119,7 @@ double MapBasedPredictionROS::getObjectYaw( } double MapBasedPredictionROS::calculateLikelihood( - const std::vector & path, - const autoware_auto_perception_msgs::msg::TrackedObject & object) + const std::vector & path, const TrackedObject & object) { // We compute the confidence value based on the object current position and angle // Calculate path length @@ -157,8 +158,7 @@ double MapBasedPredictionROS::calculateLikelihood( } bool MapBasedPredictionROS::checkCloseLaneletCondition( - const std::pair & lanelet, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std::pair & lanelet, const TrackedObject & object, const lanelet::BasicPoint2d & search_point) { // Step1. If we only have one point in the centerline, we will ignore the lanelet @@ -206,8 +206,8 @@ bool MapBasedPredictionROS::checkCloseLaneletCondition( } bool MapBasedPredictionROS::getClosestLanelets( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::LaneletMapPtr & lanelet_map_ptr_, lanelet::ConstLanelets & closest_lanelets) + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr_, + lanelet::ConstLanelets & closest_lanelets) { std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); @@ -304,11 +304,10 @@ void MapBasedPredictionROS::removeInvalidObject(const double current_time) } bool MapBasedPredictionROS::updateObjectBuffer( - const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std_msgs::msg::Header & header, const TrackedObject & object, lanelet::ConstLanelets & current_lanelets) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + using Label = ObjectClassification; // Ignore non-vehicle object if ( object.classification.front().label != Label::CAR && @@ -432,8 +431,8 @@ double MapBasedPredictionROS::calcLeftLateralOffset( } Maneuver MapBasedPredictionROS::detectLaneChange( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::ConstLanelet & current_lanelet, const double current_time) + const TrackedObject & object, const lanelet::ConstLanelet & current_lanelet, + const double current_time) { // Step1. Check if we have the object in the buffer const std::string object_id = toHexString(object.object_id); @@ -530,8 +529,7 @@ Maneuver MapBasedPredictionROS::detectLaneChange( return Maneuver::LANE_FOLLOW; } -void MapBasedPredictionROS::objectsCallback( - const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects) +void MapBasedPredictionROS::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { debug_accumulated_time_ = 0.0; std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); @@ -570,7 +568,7 @@ void MapBasedPredictionROS::objectsCallback( ///////////////////////////////////////////////////////// ///////////////////// Prediction /////////////////////// /////////////////////////////////////////////////////// - autoware_auto_perception_msgs::msg::PredictedObjects objects_without_map; + PredictedObjects objects_without_map; objects_without_map.header = in_objects->header; DynamicObjectWithLanesArray prediction_input; prediction_input.header = in_objects->header; @@ -734,22 +732,21 @@ void MapBasedPredictionROS::objectsCallback( std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); - std::vector out_objects_in_map; + std::vector out_objects_in_map; map_based_prediction_->doPrediction(prediction_input, out_objects_in_map); - autoware_auto_perception_msgs::msg::PredictedObjects output; + PredictedObjects output; output.header = in_objects->header; output.header.frame_id = "map"; output.objects = out_objects_in_map; - std::vector out_objects_without_map; + std::vector out_objects_without_map; map_based_prediction_->doLinearPrediction(objects_without_map, out_objects_without_map); output.objects.insert( output.objects.begin(), out_objects_without_map.begin(), out_objects_without_map.end()); pub_objects_->publish(output); } -void MapBasedPredictionROS::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +void MapBasedPredictionROS::mapCallback(const HADMapBin::ConstSharedPtr msg) { RCLCPP_INFO(get_logger(), "Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); @@ -757,6 +754,7 @@ void MapBasedPredictionROS::mapCallback( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); RCLCPP_INFO(get_logger(), "Map is loaded"); } +} // namespace map_based_prediction #include -RCLCPP_COMPONENTS_REGISTER_NODE(MapBasedPredictionROS) +RCLCPP_COMPONENTS_REGISTER_NODE(map_based_prediction::MapBasedPredictionROS) diff --git a/perception/tensorrt_yolo/CMakeLists.txt b/perception/tensorrt_yolo/CMakeLists.txt index c5ad50afde187..e93c6fcef104d 100755 --- a/perception/tensorrt_yolo/CMakeLists.txt +++ b/perception/tensorrt_yolo/CMakeLists.txt @@ -217,7 +217,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} mish_plugin yolo_layer_plugin diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index 4e591fa478c39..2f07b8d328cee 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -132,7 +132,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} ${Boost_LIBRARIES} ) diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt index 7fc6ad12675f2..b20d06f91f5a5 100644 --- a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -140,7 +140,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} ) 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 778e292f2310a..c368ca1011260 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -673,8 +673,8 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( const PathWithLaneId & path) const { const auto goal = planner_data_->route_handler->getGoalPose(); - const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId(); const auto is_approved = planner_data_->approval.is_approved.data; + auto goal_lane_id = planner_data_->route_handler->getGoalLaneId(); Pose refined_goal{}; { @@ -685,6 +685,7 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( is_approved && planner_data_->route_handler->getPullOverTarget( planner_data_->route_handler->getShoulderLanelets(), &pull_over_lane)) { refined_goal = planner_data_->route_handler->getPullOverGoalPose(); + goal_lane_id = pull_over_lane.id(); } else if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) { refined_goal = util::refineGoal(goal, goal_lanelet); } else { 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 8a18c6450884c..1147fce939eec 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 @@ -1733,8 +1733,8 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c } // get previous lane, and return false if previous lane does not exist - lanelet::ConstLanelet prev_lane; - if (!route_handler->getPreviousLaneletWithinRoute(lane, &prev_lane)) { + lanelet::ConstLanelets prev_lanes; + if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { return false; } 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 472d59a4d9a0d..67592fdf8a33e 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 @@ -338,8 +338,9 @@ std::pair LaneChangeModule::getSafePath( // select valid path valid_paths = lane_change_utils::selectValidPaths( - lane_change_paths, current_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, - route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose()); + lane_change_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), + current_pose, route_handler->isInGoalRouteSection(current_lanes.back()), + route_handler->getGoalPose()); if (valid_paths.empty()) { return std::make_pair(false, false); 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 a634d095f63a5..1a8701ff1672d 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 @@ -422,7 +422,7 @@ std::pair PullOutModule::getSafePath( // select valid path valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, + 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()) { @@ -493,7 +493,7 @@ std::pair PullOutModule::getSafeRetreatPath( // select valid path valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, + 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()) { @@ -574,7 +574,7 @@ bool PullOutModule::getBackDistance( // select valid path valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, + 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()) { 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 195b482708a21..06242c9e92cdf 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 @@ -353,8 +353,9 @@ std::pair PullOverModule::getSafePath( // select valid path valid_paths = pull_over_utils::selectValidPaths( - pull_over_paths, current_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, - route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose()); + pull_over_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), + current_pose, route_handler->isInGoalRouteSection(current_lanes.back()), + route_handler->getGoalPose()); if (valid_paths.empty()) { return std::make_pair(false, false); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index bdaa5adae8bb0..10ce43e18e6d5 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -352,7 +352,7 @@ PredictedPath convertToPredictedPath( { PredictedPath predicted_path{}; predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); - predicted_path.path.reserve(path.points.size()); + predicted_path.path.reserve(std::min(path.points.size(), static_cast(100))); if (path.points.empty()) { return predicted_path; } diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index c589624e56f61..927793f713433 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -224,7 +224,7 @@ ament_auto_add_library(scene_module_occlusion_spot SHARED src/scene_module/occlusion_spot/manager.cpp src/scene_module/occlusion_spot/debug.cpp src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp - src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp + src/scene_module/occlusion_spot/scene_occlusion_spot.cpp src/scene_module/occlusion_spot/occlusion_spot_utils.cpp src/scene_module/occlusion_spot/risk_predictive_braking.cpp ) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 7ccda0468bac6..a70d94e7e3e54 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: occlusion_spot: + method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" debug: false # [-] whether to publish debug markers. Note Default should be false for performance pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity threshold: @@ -18,7 +19,7 @@ safe_margin: 1.0 # [m] maximum safety distance for any error detection_area: min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. - slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. + slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. max_lateral_distance: 4.0 # [m] buffer around the ego path used to build the detection area. grid: free_space_max: 40 # [-] maximum value of a free space cell in the occupancy grid 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 da9896b898ca0..aabddf6ba6836 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 @@ -15,6 +15,8 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ #define BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ +#include "route_handler/route_handler.hpp" + #include #include @@ -69,16 +71,11 @@ struct PlannerData std::deque velocity_buffer; autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; - lanelet::LaneletMapPtr lanelet_map; // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; // other internal data std::map traffic_light_id_map; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules; - lanelet::routing::RoutingGraphPtr routing_graph; - std::shared_ptr overall_graphs; - // external data std::map external_traffic_light_id_map; @@ -86,6 +83,8 @@ struct PlannerData boost::optional external_intersection_status_input; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + // route handler + std::shared_ptr route_handler_; // parameters vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index ca09b1cd7d422..2396bb16dc8f5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -37,6 +37,21 @@ class IntersectionModuleManager : public SceneModuleManagerInterface private: IntersectionModule::PlannerParam intersection_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +{ +public: + explicit MergeFromPrivateModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "merge_from_private"; } + +private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 407d9715c65b7..e38efd3f6bc9a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -99,7 +99,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface public: struct PlannerParam { - IntersectionModule::PlannerParam intersection_param; + double decel_velocity; + double detection_area_length; + double stop_line_margin; double stop_duration_sec; }; 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 77342c1e6219a..dc647df54f533 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -52,7 +52,7 @@ bool hasDuplicatedPoint( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const IntersectionModule::PlannerParam & planner_param, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, std::vector * objective_lanelets_result, const rclcpp::Logger logger); @@ -69,8 +69,7 @@ bool getObjectiveLanelets( */ bool generateStopLine( const int lane_id, const std::vector detection_areas, - const std::shared_ptr & planner_data, - const IntersectionModule::PlannerParam & planner_param, + const std::shared_ptr & planner_data, const double stop_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); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp index 68945d2ded3b0..6464226c788b0 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface const char * getModuleName() override { return "occlusion_spot"; } private: - enum class ModuleID { PRIVATE, PUBLIC }; + enum class ModuleID { OCCUPANCY, OBJECT }; using PlannerParam = occlusion_spot_utils::PlannerParam; PlannerParam planner_param_; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index dc6349dd12793..3064cab1d23f7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -65,6 +65,7 @@ using DetectionAreaIdx = boost::optional>; namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; +enum METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT }; struct DetectionArea { @@ -96,6 +97,7 @@ struct LatLon struct PlannerParam { + METHOD method; bool debug; // [-] // parameters in yaml double detection_area_length; // [m] @@ -177,60 +179,14 @@ struct PossibleCollisionInfo lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); // Note : consider offset_from_start_to_ego and safety margin for collision here -inline void handleCollisionOffset( - std::vector & possible_collisions, double offset, double margin) -{ - for (auto & pc : possible_collisions) { - pc.arc_lane_dist_at_collision.length -= offset; - pc.arc_lane_dist_at_collision.length -= margin; - } -} - -inline double offsetFromStartToEgo( - const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx) -{ - double offset_from_ego_to_closest = 0; - for (int i = 0; i < closest_idx; i++) { - const auto & curr_p = path.points.at(i).point.pose.position; - const auto & next_p = path.points.at(i + 1).point.pose.position; - offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p); - } - const double offset_from_closest_to_target = - -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose) - .position.x; - return offset_from_ego_to_closest + offset_from_closest_to_target; -} - -inline void clipPathByLength( - const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0) -{ - double length_sum = 0; - for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { - length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (length_sum > max_length) return; - clipped.points.emplace_back(path.points.at(i)); - } -} - -inline bool isStuckVehicle(PredictedObject obj, const double min_vel) -{ - if ( - obj.classification.at(0).label == ObjectClassification::CAR || - obj.classification.at(0).label == ObjectClassification::TRUCK || - obj.classification.at(0).label == ObjectClassification::BUS) { - if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { - return true; - } - } - return false; -} -void filterCollisionByRoadType( - std::vector & possible_collisions, const DetectionAreaIdx road_type); +void handleCollisionOffset(std::vector & possible_collisions, double offset); +void clipPathByLength( + const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0); +bool isStuckVehicle(PredictedObject obj, const double min_vel); +double offsetFromStartToEgo( + const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx); std::vector filterDynamicObjectByDetectionArea( std::vector & objs, const std::vector polys); -bool splineInterpolate( - const PathWithLaneId & input, const double interval, PathWithLaneId * output, - const rclcpp::Logger logger); std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp index 59145de966dfe..9156d5d04ce07 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp @@ -34,87 +34,25 @@ int insertSafeVelocityToPath( const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, PathWithLaneId * inout_path); -// @brief calculates the maximum velocity allowing to decelerate within the given distance -inline double calculateMinSlowDownVelocity( - const double v0, const double len, const double a_max, const double safe_vel) -{ - // if target velocity is inserted backward return current velocity as limit - if (len < 0) return safe_vel; - return std::sqrt(std::max(std::pow(v0, 2.0) - 2.0 * std::abs(a_max) * len, 0.0)); -} - /** * * @param: longitudinal_distance: longitudinal distance to collision * @param: param: planner param * @return lateral distance **/ -inline double calculateLateralDistanceFromTTC( - const double longitudinal_distance, const PlannerParam & param) -{ - const auto & v = param.v; - const auto & p = param; - double v_min = 1.0; - const double lateral_buffer = 0.5; - const double min_distance = p.half_vehicle_width + lateral_buffer; - const double max_distance = p.detection_area.max_lateral_distance; - if (longitudinal_distance <= 0) return min_distance; - if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity; - // use min velocity if ego velocity is below min allowed - const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min; - // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ? - double t = longitudinal_distance / v0; - double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width; - return std::min(max_distance, std::max(min_distance, lateral_distance)); -} +double calculateLateralDistanceFromTTC( + const double longitudinal_distance, const PlannerParam & param); /** * @param: v: ego velocity config * @param: ttc: time to collision * @return safe motion **/ -inline SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) -{ - SafeMotion sm; - const double j_max = v.safety_ratio * v.max_stop_jerk; - const double a_max = v.safety_ratio * v.max_stop_accel; - const double t1 = v.delay_time; - double t2 = a_max / j_max; - double & v_safe = sm.safe_velocity; - double & stop_dist = sm.stop_dist; - if (ttc <= t1) { - // delay - v_safe = 0; - stop_dist = 0; - } else if (ttc <= t2 + t1) { - // delay + const jerk - t2 = ttc - t1; - v_safe = -0.5 * j_max * t2 * t2; - stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6; - } else { - const double t3 = ttc - t2 - t1; - // delay + const jerk + const accel - const double v2 = -0.5 * j_max * t2 * t2; - v_safe = v2 - a_max * t3; - stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3; - } - stop_dist += v.safe_margin; - return sm; -} +SafeMotion calculateSafeMotion(const Velocity & v, const double ttc); -inline double calculateInsertVelocity( +double calculateInsertVelocity( const double min_allowed_vel, const double safe_vel, const double min_vel, - const double original_vel) -{ - const double max_vel_noise = 0.05; - // ensure safe velocity doesn't exceed maximum allowed pbs deceleration - double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel); - // ensure safe path velocity is also above ego min velocity - cmp_safe_vel = std::max(cmp_safe_vel, min_vel); - // ensure we only lower the original velocity (and do not increase it) - cmp_safe_vel = std::min(cmp_safe_vel, original_vel); - return cmp_safe_vel; -} + const double original_vel); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp similarity index 87% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp rename to planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index fed53518eb1ca..147c1e4396201 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ +#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ #include #include @@ -37,7 +37,7 @@ namespace behavior_velocity_planner { -class OcclusionSpotInPrivateModule : public SceneModuleInterface +class OcclusionSpotModule : public SceneModuleInterface { using PlannerParam = occlusion_spot_utils::PlannerParam; @@ -45,7 +45,7 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface struct DebugData { double z; - std::string road_type = "private"; + std::string road_type = "occupancy"; std::vector detection_areas; std::vector possible_collisions; std::vector occlusion_points; @@ -53,7 +53,7 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface PathWithLaneId interp_path; }; - OcclusionSpotInPrivateModule( + OcclusionSpotModule( const int64_t module_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, @@ -82,4 +82,4 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ +#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp index 869084e7ed410..073d3bb6aea86 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -43,7 +43,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface public: struct DebugData { - std::string road_type = "public"; + std::string road_type = "object"; std::vector detection_areas; std::vector parked_vehicle_point; std::vector possible_collisions; diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 2c835429311e8..a69c2b04733a9 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -27,6 +27,7 @@ pcl_conversions rclcpp rclcpp_components + route_handler sensor_msgs tf2 tf2_eigen diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 0fba77d1c406a..9bc3c94624a96 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -135,7 +135,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchSceneModule(std::make_shared(*this)); } if (this->declare_parameter("launch_intersection", true)) { + // intersection module should be before merge from private to declare intersection parameters planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchSceneModule(std::make_shared(*this)); } if (this->declare_parameter("launch_blind_spot", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); @@ -178,7 +180,10 @@ bool BehaviorVelocityPlannerNode::isDataReady() if (!d.no_ground_pointcloud) { return false; } - if (!d.lanelet_map) { + if (!d.route_handler_) { + return false; + } + if (!d.route_handler_->isMapMsgReady()) { return false; } @@ -250,32 +255,7 @@ void BehaviorVelocityPlannerNode::onLaneletMap( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { // Load map - planner_data_.lanelet_map = std::make_shared(); - lanelet::utils::conversion::fromBinMsg( - *msg, planner_data_.lanelet_map, &planner_data_.traffic_rules, &planner_data_.routing_graph); - - // Build graph - { - using lanelet::Locations; - using lanelet::Participants; - using lanelet::routing::RoutingGraph; - using lanelet::routing::RoutingGraphConstPtr; - using lanelet::routing::RoutingGraphContainer; - using lanelet::traffic_rules::TrafficRulesFactory; - - const auto traffic_rules = - TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle); - const auto pedestrian_rules = - TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian); - - RoutingGraphConstPtr vehicle_graph = - RoutingGraph::build(*planner_data_.lanelet_map, *traffic_rules); - RoutingGraphConstPtr pedestrian_graph = - RoutingGraph::build(*planner_data_.lanelet_map, *pedestrian_rules); - RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); - - planner_data_.overall_graphs = std::make_shared(overall_graphs); - } + planner_data_.route_handler_ = std::make_shared(*msg); } void BehaviorVelocityPlannerNode::onTrafficSignals( diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp index f4edbc1b737a2..6189c003eb926 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp @@ -70,7 +70,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) void BlindSpotModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & ll : getLaneletsOnPath(path, planner_data_->lanelet_map)) { + for (const auto & ll : + getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { const auto lane_id = ll.id(); const auto module_id = lane_id; diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 3070e4b4ed82f..b0e8d4c8b74b8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -45,7 +45,8 @@ BlindSpotModule::BlindSpotModule( turn_direction_(TurnDirection::INVALID) { planner_param_ = planner_param; - const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + const auto & assigned_lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); if (!turn_direction.compare("left")) { turn_direction_ = TurnDirection::LEFT; @@ -74,8 +75,8 @@ bool BlindSpotModule::modifyPathVelocity( geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->lanelet_map; - const auto routing_graph_ptr = planner_data_->routing_graph; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* set stop-line and stop-judgement-line for base_link */ int stop_line_idx = -1; @@ -518,7 +519,8 @@ bool BlindSpotModule::isTargetObjectType( boost::optional BlindSpotModule::getStartPointFromLaneLet( const int lane_id) const { - lanelet::ConstLanelet lanelet = planner_data_->lanelet_map->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); if (lanelet.centerline().empty()) { return boost::none; } 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 d4425eb8ff917..52d2da760d575 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -90,8 +90,9 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) void CrosswalkModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { + const auto rh = planner_data_->route_handler_; for (const auto & crosswalk : - getCrosswalksOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs)) { + getCrosswalksOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr())) { const auto module_id = crosswalk.id(); if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( @@ -112,8 +113,9 @@ std::function &)> CrosswalkModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { + const auto rh = planner_data_->route_handler_; const auto crosswalk_id_set = - getCrosswalkIdSetOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs); + getCrosswalkIdSetOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp index 6fc8498a79d4a..1f6b8d9a9eb46 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp @@ -175,7 +175,8 @@ lanelet::Optional getStopLineFromMap( const int lane_id, const std::shared_ptr & planner_data, const std::string & attribute_name) { - lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); const auto road_markings = lanelet.regulatoryElementsAs(); lanelet::ConstLineStrings3d stop_line; for (const auto & road_marking : road_markings) { 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 f07106d6ea1af..8dc92269a4e1d 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 @@ -73,7 +73,7 @@ void DetectionAreaModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { for (const auto & detection_area : - getDetectionAreaRegElemsOnPath(path, planner_data_->lanelet_map)) { + getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = detection_area->id(); if (!isModuleRegistered(module_id)) { @@ -88,7 +88,8 @@ std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto detection_area_id_set = getDetectionAreaIdSetOnPath(path, planner_data_->lanelet_map); + const auto detection_area_id_set = + getDetectionAreaIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [detection_area_id_set](const std::shared_ptr & scene_module) { return detection_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index d346856e8eadc..13e06e2dfe7cc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -150,7 +150,8 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray( - const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor) + const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor, + const double offset_z = 0.0) { visualization_msgs::msg::MarkerArray msg; @@ -175,7 +176,7 @@ visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray( marker_factor_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_factor_text.action = visualization_msgs::msg::Marker::ADD; marker_factor_text.pose = pose; - marker_factor_text.pose.position.z += 2.0; + marker_factor_text.pose.position.z += 2.0 + offset_z; marker_factor_text.scale = createMarkerScale(0.0, 0.0, 1.0); marker_factor_text.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); marker_factor_text.text = stop_factor; @@ -345,7 +346,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark appendMarkerArray( createVirtualStopWallMarkerArray( - debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road"), + debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road", -1.0), current_time, &debug_marker_array); } 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 0d235d339af54..b46f18a139a70 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -83,16 +83,50 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0); ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0); +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); auto & mp = merge_from_private_area_param_; mp.stop_duration_sec = node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0); - mp.intersection_param = intersection_param_; + mp.decel_velocity = node.get_parameter("intersection.decel_velocity").as_double(); + mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double(); + mp.stop_line_margin = node.get_parameter("intersection.stop_line_margin").as_double(); } void IntersectionModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map); + const auto lanelets = getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + registerModule(std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, + logger_.get_child("intersection_module"), clock_)); + } +} + +void MergeFromPrivateModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lanelets = getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -121,10 +155,6 @@ void IntersectionModuleManager::launchNewModules( logger_.get_child("merge_from_private_road_module"), clock_)); } } - - registerModule(std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, - logger_.get_child("intersection_module"), clock_)); } } @@ -138,4 +168,14 @@ IntersectionModuleManager::getModuleExpiredFunction( return lane_id_set.count(scene_module->getModuleId()) == 0; }; } +std::function &)> +MergeFromPrivateModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_id_set = getLaneIdSetOnPath(path); + + return [lane_id_set](const std::shared_ptr & scene_module) { + return lane_id_set.count(scene_module->getModuleId()) == 0; + }; +} } // namespace behavior_velocity_planner 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 5ea098e885a56..9e2e86ccfe421 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 @@ -56,7 +56,8 @@ IntersectionModule::IntersectionModule( : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) { planner_param_ = planner_param; - const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + const auto & assigned_lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); has_traffic_light_ = !(assigned_lanelet.regulatoryElementsAs().empty()); @@ -85,16 +86,16 @@ bool IntersectionModule::modifyPathVelocity( geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->lanelet_map; - const auto routing_graph_ptr = planner_data_->routing_graph; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area and conflicting area */ std::vector detection_area_lanelets; std::vector conflicting_area_lanelets; util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_, &conflicting_area_lanelets, - &detection_area_lanelets, logger_); + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLaneletsVec( @@ -115,8 +116,8 @@ bool IntersectionModule::modifyPathVelocity( int pass_judge_line_idx = -1; int first_idx_inside_lane = -1; if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_, 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, path, *path, + &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); return false; @@ -603,7 +604,7 @@ bool IntersectionModule::checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids) { for (const int lanelet_id : target_lanelet_ids) { - const auto ll = planner_data_->lanelet_map->laneletLayer.get(lanelet_id); + const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id); if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) { continue; } 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 ceccb05d2659b..92f42ed7a2905 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 @@ -61,18 +61,18 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->lanelet_map; - const auto routing_graph_ptr = planner_data_->routing_graph; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area */ std::vector detection_area_lanelets; std::vector conflicting_area_lanelets; util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.intersection_param, + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( - conflicting_area_lanelets, planner_param_.intersection_param.detection_area_length); + conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { RCLCPP_DEBUG(logger_, "no detection area. skip computation."); return true; @@ -86,7 +86,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.intersection_param, path, + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); @@ -108,7 +108,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* set stop speed */ if (state_machine_.getState() == State::STOP) { constexpr double stop_vel = 0.0; - const double decel_vel = planner_param_.intersection_param.decel_velocity; + const double decel_vel = planner_param_.decel_velocity; double v = (has_traffic_light_ && turn_direction_ == "straight") ? decel_vel : stop_vel; util::setVelocityFrom(stop_line_idx, v, path); 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 95946a6ba61be..b0a91cba575f3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -162,8 +162,7 @@ int getFirstPointInsidePolygons( bool generateStopLine( const int lane_id, const std::vector detection_areas, - const std::shared_ptr & planner_data, - const IntersectionModule::PlannerParam & planner_param, + const std::shared_ptr & planner_data, const double stop_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) @@ -178,7 +177,7 @@ bool generateStopLine( /* set parameters */ constexpr double interval = 0.2; - const int margin_idx_dist = std::ceil(planner_param.stop_line_margin / interval); + const int margin_idx_dist = std::ceil(stop_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); @@ -266,7 +265,8 @@ bool getStopPoseIndexFromMap( const std::shared_ptr & planner_data, int & stop_idx_ip, int dist_thr, const rclcpp::Logger logger) { - lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); const auto road_markings = lanelet.regulatoryElementsAs(); lanelet::ConstLineStrings3d stop_line; for (const auto & road_marking : road_markings) { @@ -322,7 +322,7 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const IntersectionModule::PlannerParam & planner_param, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, std::vector * objective_lanelets_result, const rclcpp::Logger logger) { @@ -379,7 +379,7 @@ bool getObjectiveLanelets( } // get possible lanelet path that reaches conflicting_lane longer than given length - const double length = planner_param.detection_area_length; + const double length = detection_area_length; std::vector objective_lanelets_sequences; for (const auto & ll : objective_lanelets) { // Preceding lanes does not include objective_lane so add them at the end 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 df372e73e1406..82e4211f52c6f 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 @@ -80,7 +80,7 @@ void NoStoppingAreaModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { for (const auto & no_stopping_area : - getNoStoppingAreaRegElemsOnPath(path, planner_data_->lanelet_map)) { + getNoStoppingAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = no_stopping_area->id(); if (!isModuleRegistered(module_id)) { @@ -97,7 +97,7 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = - getNoStoppingAreaIdSetOnPath(path, planner_data_->lanelet_map); + getNoStoppingAreaIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 4cae4d7b92a80..595d97d766a94 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include @@ -269,7 +269,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar return debug_marker_array; } -visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMarkerArray() +visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray() { const auto current_time = this->clock_->now(); 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 6e7d31e4b5705..8e8b408082e78 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 @@ -16,6 +16,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -181,13 +182,14 @@ void toQuantizedImage( for (int y = height - 1; y >= 0; y--) { const int idx = (height - 1 - y) + (width - 1 - x) * height; int8_t intensity = occupancy_grid.data.at(idx); - if (0 <= intensity && intensity < param.free_space_max) { + if (0 <= intensity && intensity <= param.free_space_max) { intensity = grid_utils::occlusion_cost_value::FREE_SPACE; - } else if ( // NOLINT - intensity == occlusion_cost_value::NO_INFORMATION || intensity < param.occupied_min) { + } else if (param.free_space_max < intensity && intensity < param.occupied_min) { intensity = grid_utils::occlusion_cost_value::UNKNOWN; - } else { + } else if (param.occupied_min <= intensity) { intensity = grid_utils::occlusion_cost_value::OCCUPIED; + } else { + std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause"); } cv_image->at(y, x) = intensity; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp index 1ad0f5970c3f2..9c57cff00f35d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include @@ -25,50 +25,7 @@ namespace behavior_velocity_planner { -namespace -{ -std::vector getLaneletsOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - std::vector lanelets; - - for (const auto & p : path.points) { - const auto lane_id = p.lane_ids.at(0); - lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); - } - return lanelets; -} - -bool hasPublicRoadOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { - // Is public load ? - const std::string location = ll.attributeOr("location", "else"); - if (location == "urban" || location == "public") { - return true; - } - } - return false; -} - -bool hasPrivateRoadOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { - // Is private load ? - const std::string location = ll.attributeOr("location", "else"); - if (location == "private") { - return true; - } - } - return false; -} - -} // namespace +using occlusion_spot_utils::METHOD; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -80,6 +37,14 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & pp = planner_param_; // assume pedestrian coming out from occlusion spot with this velocity + const std::string method = node.declare_parameter(ns + ".method", "occupancy_grid"); + if (method == "occupancy_grid") { + pp.method = METHOD::OCCUPANCY_GRID; + } else if (method == "predicted_object") { + pp.method = METHOD::PREDICTED_OBJECT; + } else { + throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"}; + } pp.debug = node.declare_parameter(ns + ".debug", false); pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0); pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0); @@ -117,19 +82,20 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) void OcclusionSpotModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const int64_t private_road_module_id = static_cast(ModuleID::PRIVATE); - const int64_t public_road_module_id = static_cast(ModuleID::PUBLIC); - // private - if (!isModuleRegistered(private_road_module_id)) { - if (hasPrivateRoadOnPath(path, planner_data_->lanelet_map)) { - registerModule(std::make_shared( - private_road_module_id, planner_data_, planner_param_, - logger_.get_child("occlusion_spot_in_private_module"), clock_, pub_debug_occupancy_grid_)); + if (path.points.empty()) return; + const int64_t module_id = static_cast(ModuleID::OCCUPANCY); + const int64_t public_road_module_id = static_cast(ModuleID::OBJECT); + // general + if (!isModuleRegistered(module_id)) { + if (planner_param_.method == METHOD::OCCUPANCY_GRID) { + registerModule(std::make_shared( + module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), + clock_, pub_debug_occupancy_grid_)); } } // public if (!isModuleRegistered(public_road_module_id)) { - if (hasPublicRoadOnPath(path, planner_data_->lanelet_map)) { + if (planner_param_.method == METHOD::PREDICTED_OBJECT) { registerModule(std::make_shared( public_road_module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_in_public_module"), clock_)); @@ -141,17 +107,8 @@ std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const bool has_public_road = hasPublicRoadOnPath(path, planner_data_->lanelet_map); - const bool has_private_road = hasPrivateRoadOnPath(path, planner_data_->lanelet_map); - return [has_private_road, - has_public_road](const std::shared_ptr & scene_module) { - if (scene_module->getModuleId() == static_cast(ModuleID::PRIVATE)) { - return !has_private_road; - } - if (scene_module->getModuleId() == static_cast(ModuleID::PUBLIC)) { - return !has_public_road; - } - return true; + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + return false; }; } } // namespace behavior_velocity_planner 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 002306a91e7f6..8ba933fd2db40 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 @@ -146,6 +146,52 @@ void calcSlowDownPointsForPossibleCollision( } } +void handleCollisionOffset(std::vector & possible_collisions, double offset) +{ + for (auto & pc : possible_collisions) { + pc.arc_lane_dist_at_collision.length -= offset; + } +} + +void clipPathByLength( + const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length) +{ + double length_sum = 0; + for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { + length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + if (length_sum > max_length) return; + clipped.points.emplace_back(path.points.at(i)); + } +} + +bool isStuckVehicle(PredictedObject obj, const double min_vel) +{ + if ( + obj.classification.at(0).label == ObjectClassification::CAR || + obj.classification.at(0).label == ObjectClassification::TRUCK || + obj.classification.at(0).label == ObjectClassification::BUS) { + if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { + return true; + } + } + return false; +} + +double offsetFromStartToEgo( + const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx) +{ + double offset_from_ego_to_closest = 0; + for (int i = 0; i < closest_idx; i++) { + const auto & curr_p = path.points.at(i).point.pose.position; + const auto & next_p = path.points.at(i + 1).point.pose.position; + offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p); + } + const double offset_from_closest_to_target = + -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose) + .position.x; + return offset_from_ego_to_closest + offset_from_closest_to_target; +} + std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point) @@ -337,21 +383,6 @@ std::vector filterDynamicObjectByDetectionArea( return filtered_obj; } -void filterCollisionByRoadType( - std::vector & possible_collisions, const DetectionAreaIdx area) -{ - std::pair focus_length = area.get(); - for (auto it = possible_collisions.begin(); it != possible_collisions.end();) { - const auto & pc_len = it->arc_lane_dist_at_collision.length; - if (focus_length.first < pc_len && pc_len < focus_length.second) { - it++; - } else { - // -----erase-----|start------target-------end|----erase--- - it = possible_collisions.erase(it); - } - } -} - void createPossibleCollisionsInDetectionArea( const std::vector & detection_area_polygons, std::vector & possible_collisions, const grid_map::GridMap & grid, diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index e2427b90ae055..cb7e56245a4c4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -47,7 +47,7 @@ void applySafeVelocityConsideringPossibleCollision( // min allowed velocity : min allowed velocity consider maximum allowed braking const double v_slow_down = - (l_obs < 0) + (l_obs < 0 && v0 <= v_safe) ? v_safe : planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs); // compare safe velocity consider EBS, minimum allowed velocity and original velocity @@ -105,5 +105,66 @@ int insertSafeVelocityToPath( return 0; } +double calculateLateralDistanceFromTTC( + const double longitudinal_distance, const PlannerParam & param) +{ + const auto & v = param.v; + const auto & p = param; + double v_min = 1.0; + const double lateral_buffer = 0.5; + const double min_distance = p.half_vehicle_width + lateral_buffer; + const double max_distance = p.detection_area.max_lateral_distance; + if (longitudinal_distance <= 0) return min_distance; + if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity; + // use min velocity if ego velocity is below min allowed + const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min; + // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ? + double t = longitudinal_distance / v0; + double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width; + return std::min(max_distance, std::max(min_distance, lateral_distance)); +} + +SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) +{ + SafeMotion sm; + const double j_max = v.safety_ratio * v.max_stop_jerk; + const double a_max = v.safety_ratio * v.max_stop_accel; + const double t1 = v.delay_time; + double t2 = a_max / j_max; + double & v_safe = sm.safe_velocity; + double & stop_dist = sm.stop_dist; + if (ttc <= t1) { + // delay + v_safe = 0; + stop_dist = 0; + } else if (ttc <= t2 + t1) { + // delay + const jerk + t2 = ttc - t1; + v_safe = -0.5 * j_max * t2 * t2; + stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6; + } else { + const double t3 = ttc - t2 - t1; + // delay + const jerk + const accel + const double v2 = -0.5 * j_max * t2 * t2; + v_safe = v2 - a_max * t3; + stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3; + } + stop_dist += v.safe_margin; + return sm; +} + +double calculateInsertVelocity( + const double min_allowed_vel, const double safe_vel, const double min_vel, + const double original_vel) +{ + const double max_vel_noise = 0.05; + // ensure safe velocity doesn't exceed maximum allowed pbs deceleration + double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel); + // ensure safe path velocity is also above ego min velocity + cmp_safe_vel = std::max(cmp_safe_vel, min_vel); + // ensure we only lower the original velocity (and do not increase it) + cmp_safe_vel = std::min(cmp_safe_vel, original_vel); + return cmp_safe_vel; +} } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp similarity index 84% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp rename to planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 4ce2b34b60c60..426a2efbfbf6e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include @@ -27,12 +27,11 @@ namespace behavior_velocity_planner { -using occlusion_spot_utils::ROAD_TYPE::PRIVATE; namespace bg = boost::geometry; namespace lg = lanelet::geometry; namespace utils = occlusion_spot_utils; -OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( +OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, @@ -42,12 +41,11 @@ OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( param_ = planner_param; } -bool OcclusionSpotInPrivateModule::modifyPathVelocity( +bool OcclusionSpotModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); - debug_data_.road_type = "private"; if (path->points.size() < 2) { return true; } @@ -62,10 +60,8 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( 0.0); } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; - const auto & lanelet_map_ptr = planner_data_->lanelet_map; - const auto & traffic_rules_ptr = planner_data_->traffic_rules; const auto & occ_grid_ptr = planner_data_->occupancy_grid; - if (!lanelet_map_ptr || !traffic_rules_ptr || !occ_grid_ptr) { + if (!occ_grid_ptr) { return true; } @@ -83,9 +79,6 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( } // return if ego is final point of interpolated path if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - DetectionAreaIdx focus_area = - extractTargetRoadArcLength(lanelet_map_ptr, max_range, *path, PRIVATE); - if (!focus_area) return true; nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; grid_map::GridMap grid_map; grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); @@ -94,21 +87,17 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( std::vector detection_area_polygons; utils::buildDetectionAreaPolygon( detection_area_polygons, interp_path, offset_from_start_to_ego, param_); - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 3000, "offset_from_start_to_ego : " << offset_from_start_to_ego); std::vector possible_collisions; // Note: Don't consider offset from path start to ego here utils::createPossibleCollisionsInDetectionArea( detection_area_polygons, possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_.occlusion_points); if (detection_area_polygons.empty()) return true; - utils::filterCollisionByRoadType(possible_collisions, focus_area); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here - utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); // these debug topics needs computation resource diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index 15b5029d39b3f..a351bce4a62c3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -30,7 +30,6 @@ namespace behavior_velocity_planner { using occlusion_spot_utils::PossibleCollisionInfo; -using occlusion_spot_utils::ROAD_TYPE::PUBLIC; namespace utils = occlusion_spot_utils; OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( @@ -47,7 +46,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); - debug_data_.road_type = "public"; if (path->points.size() < 2) { return true; } @@ -62,12 +60,9 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( 0.0); } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; - const auto & lanelet_map_ptr = planner_data_->lanelet_map; - const auto & routing_graph_ptr = planner_data_->routing_graph; - const auto & traffic_rules_ptr = planner_data_->traffic_rules; const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - if (!lanelet_map_ptr || !traffic_rules_ptr || !dynamic_obj_arr_ptr || !routing_graph_ptr) { + if (!dynamic_obj_arr_ptr) { return true; } PathWithLaneId clipped_path; @@ -81,9 +76,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( } // return if ego is final point of interpolated path if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - DetectionAreaIdx focus_area = - extractTargetRoadArcLength(lanelet_map_ptr, param_.detection_area_length, *path, PUBLIC); - if (!focus_area) return true; std::vector obj = utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); @@ -96,10 +88,9 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( std::vector possible_collisions = utils::generatePossibleCollisionBehindParkedVehicle( interp_path, param_, offset_from_start_to_ego, filtered_obj); - utils::filterCollisionByRoadType(possible_collisions, focus_area); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here - utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); if (param_.debug) { 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 ebb038c191f85..7c78395470e73 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 @@ -94,7 +94,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) void StopLineModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & stop_line : getStopLinesOnPath(path, planner_data_->lanelet_map)) { + for (const auto & stop_line : + getStopLinesOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { const auto module_id = stop_line.id(); if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( @@ -107,7 +108,8 @@ std::function &)> StopLineModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->lanelet_map); + const auto stop_line_id_set = + getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [stop_line_id_set](const std::shared_ptr & scene_module) { return stop_line_id_set.count(scene_module->getModuleId()) == 0; 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 bb82168f170dd..2b2d8dff8b36a 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 @@ -123,7 +123,7 @@ void TrafficLightModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { for (const auto & traffic_light_reg_elem : - getTrafficLightRegElemsOnPath(path, planner_data_->lanelet_map)) { + getTrafficLightRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { const auto stop_line = traffic_light_reg_elem.first->stopLine(); if (!stop_line) { @@ -147,7 +147,8 @@ std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lanelet_id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map); + const auto lanelet_id_set = + getLaneletIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [lanelet_id_set](const std::shared_ptr & scene_module) { return lanelet_id_set.count(scene_module->getModuleId()) == 0; 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 c637ffbd73ef0..7ac6918f13ed2 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 @@ -81,8 +81,8 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & m : - getRegElemMapOnPath(path, planner_data_->lanelet_map)) { + for (const auto & m : getRegElemMapOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = m.second.id(); if (!isModuleRegistered(module_id)) { @@ -97,7 +97,8 @@ std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map); + const auto id_set = + getLaneletIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 8eedd18786a4f..7cf38a40d0ee2 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -73,7 +73,13 @@ class RouteHandler bool isHandlerReady() const; lanelet::ConstPolygon3d getExtraDrivableAreaById(const lanelet::Id id) const; Header getRouteHeader() const; - lanelet::routing::RoutingGraphContainer getOverallGraph() const; + + // for routing graph + bool isMapMsgReady() const; + lanelet::routing::RoutingGraphPtr getRoutingGraphPtr() const; + lanelet::traffic_rules::TrafficRulesPtr getTrafficRulesPtr() const; + std::shared_ptr getOverallGraphPtr() const; + lanelet::LaneletMapPtr getLaneletMapPtr() const; // for routing bool planPathLaneletsBetweenCheckpoints( @@ -90,8 +96,8 @@ class RouteHandler std::vector getLanesAfterGoal(const double vehicle_length) const; // for lanelet - bool getPreviousLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const; + bool getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index be4a14173f7e1..ac48a20ea0209 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -495,15 +495,39 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelet current_lanelet = lanelet; double length = 0; - while (rclcpp::ok() && length < min_length) { - lanelet::ConstLanelet prev_lanelet; - if (!getPreviousLaneletWithinRoute(current_lanelet, &prev_lanelet)) { + lanelet::ConstLanelets candidate_lanelets; + if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { + break; + } + + // If lanelet_sequence_backward with input lanelet contains all candidate lanelets, + // break the loop. + if (std::all_of( + candidate_lanelets.begin(), candidate_lanelets.end(), + [lanelet_sequence_backward, lanelet](auto & prev_llt) { + return std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_llt, lanelet](auto & llt) { + return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); + }); + })) { + break; + } + + for (const auto & prev_lanelet : candidate_lanelets) { + if (std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_lanelet, lanelet](auto & llt) { + return (llt.id() == prev_lanelet.id() || lanelet.id() == prev_lanelet.id()); + })) { + continue; + } + lanelet_sequence_backward.push_back(prev_lanelet); + length += boost::geometry::length(prev_lanelet.centerline().basicLineString()); + current_lanelet = prev_lanelet; break; } - lanelet_sequence_backward.push_back(prev_lanelet); - length += boost::geometry::length(prev_lanelet.centerline().basicLineString()); - current_lanelet = prev_lanelet; } std::reverse(lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); @@ -744,20 +768,20 @@ lanelet::ConstLanelets RouteHandler::getNextLanelets(const lanelet::ConstLanelet return routing_graph_ptr_->following(lanelet); } -bool RouteHandler::getPreviousLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const +bool RouteHandler::getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const { if (exists(start_lanelets_, lanelet)) { return false; } - lanelet::ConstLanelets previous_lanelets = routing_graph_ptr_->previous(lanelet); - for (const auto & llt : previous_lanelets) { + lanelet::ConstLanelets candidate_lanelets = routing_graph_ptr_->previous(lanelet); + prev_lanelets->clear(); + for (const auto & llt : candidate_lanelets) { if (exists(route_lanelets_, llt)) { - *prev_lanelet = llt; - return true; + prev_lanelets->push_back(llt); } } - return false; + return !(prev_lanelets->empty()); } lanelet::ConstLanelets RouteHandler::getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const @@ -1271,11 +1295,26 @@ lanelet::ConstLanelets RouteHandler::getCheckTargetLanesFromPath( return check_lanelets; } -lanelet::routing::RoutingGraphContainer RouteHandler::getOverallGraph() const +bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; } + +lanelet::routing::RoutingGraphPtr RouteHandler::getRoutingGraphPtr() const +{ + return routing_graph_ptr_; +} + +lanelet::traffic_rules::TrafficRulesPtr RouteHandler::getTrafficRulesPtr() const +{ + return traffic_rules_ptr_; +} + +std::shared_ptr RouteHandler::getOverallGraphPtr() + const { - return *overall_graphs_ptr_; + return overall_graphs_ptr_; } +lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const { return lanelet_map_ptr_; } + lanelet::routing::RelationType RouteHandler::getRelation( const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const { @@ -1359,15 +1398,42 @@ lanelet::ConstLanelets RouteHandler::getLaneSequence(const lanelet::ConstLanelet lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo( const lanelet::ConstLanelet & lanelet) const { - lanelet::ConstLanelets lane_sequence_backward; + lanelet::ConstLanelets lanelet_sequence_backward; if (!exists(route_lanelets_, lanelet)) { - return lane_sequence_backward; + return lanelet_sequence_backward; } lanelet::ConstLanelet current_lanelet = lanelet; while (rclcpp::ok()) { + lanelet::ConstLanelets candidate_lanelets; + if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { + break; + } + + // If lanelet_sequence_backward with input lanelet contains all candidate lanelets, + // break the loop. + if (std::all_of( + candidate_lanelets.begin(), candidate_lanelets.end(), + [lanelet_sequence_backward, lanelet](auto & prev_llt) { + return std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_llt, lanelet](auto & llt) { + return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); + }); + })) { + break; + } + lanelet::ConstLanelet prev_lanelet; - if (!getPreviousLaneletWithinRoute(current_lanelet, &prev_lanelet)) { + for (const auto & prev_llt : candidate_lanelets) { + if (std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_llt, lanelet](auto & llt) { + return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); + })) { + continue; + } + prev_lanelet = prev_llt; break; } @@ -1376,12 +1442,12 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo( if (!isBijectiveConnection(prev_lanelet_section, current_lanelet_section)) { break; } - lane_sequence_backward.push_back(prev_lanelet); + lanelet_sequence_backward.push_back(prev_lanelet); current_lanelet = prev_lanelet; } - std::reverse(lane_sequence_backward.begin(), lane_sequence_backward.end()); - return lane_sequence_backward; + std::reverse(lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); + return lanelet_sequence_backward; } lanelet::ConstLanelets RouteHandler::getLaneSequenceAfter( diff --git a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py index fc7b9323ad183..9b550c359a9ab 100644 --- a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py +++ b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py @@ -61,7 +61,7 @@ def add_launch_arg(name: str, default_value=None): "max_height": 2.0, "angle_min": -3.141592, # -M_PI "angle_max": 3.141592, # M_PI - "angle_increment": 0.00436332222, # 0.25*M_PI/180.0 + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 "scan_time": 0.0, "range_min": 1.0, "range_max": 200.0, diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md index d2ddc3315256b..9cf07746b075d 100644 --- a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -14,11 +14,22 @@ Therefore, in order to use this node, the sensor driver must publish custom data Another feature of this node is that it publishes visibility as a diagnostic topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle. +In some complicated road scenes where normal objects also reflect the light in two stages, for instance plants, leaves, some plastic net etc, the visibility faces some drop in fine weather condition. To deal with that, optional settings of a region of interest (ROI) are added. + +1. `Fixed_xyz_ROI` mode: Visibility estimation based on the weak points in a fixed cuboid surrounding region of ego-vehicle, defined by x, y, z in base_link perspective. +2. `Fixed_azimuth_ROI` mode: Visibility estimation based on the weak points in a fixed surrounding region of ego-vehicle, defined by azimuth and distance of LiDAR perspective. + +When select 2 fixed ROI modes, due to the range of weak points is shrink, the sensitivity of visibility is decrease so that a trade of between `weak_first_local_noise_threshold` and `visibility_threshold` is needed. + ![outlier_filter-dual_return_overall](./image/outlier_filter-dual_return_overall.drawio.svg) The figure below describe how the node works. ![outlier_filter-dual_return_detail](./image/outlier_filter-dual_return_detail.drawio.svg) +The below picture shows the ROI options. + +![outlier_filter-dual_return_ROI_setting_options](./image/outlier_filter-dual_return_ROI_setting_options.png) + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -46,7 +57,18 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter | | `general_distance_ratio` | double | Threshold for ring_outlier_filter | | `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | -| `visibility_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | +| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | +| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | +| `roi_mode` | string | The name of ROI mode for switching | +| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | +| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | +| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode | +| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode | +| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode | +| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode | +| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode | +| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode | +| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png b/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png new file mode 100644 index 0000000000000..aad1eb144786f Binary files /dev/null and b/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png differ diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg b/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg index 7ef64eba8e84c..b5c01fa3c6b64 100644 --- a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg +++ b/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg @@ -1,4 +1,4 @@ -
input pointcloud (PointXYZIRADT)
input pointcloud (Poi...
split  points by return_type
split  points by r...
ring outlier filter
ring outlier filter
weak points
weak points
normal points
normal points
ring outlier filter
ring outlier filter
combine
combine
good points
good points
create histogram
create histogram
noise points
noise points
diagnostics
diagnostics
create binary histogram
create binary hist...
estimate visibility
estimate visibility
output pointcloud (PointXYZIRADT)
output pointcloud...Viewer does not support full SVG 1.1 \ No newline at end of file +
input pointcloud (PointXYZIRADT)
input pointcloud (Poi...
split  points by return_type
split  points by r...
ring outlier filter
ring outlier filter
weak points
weak points
normal points
normal points
ring outlier filter
ring outlier filter
combine
combine
good points
good points
create histogram
create histogram
noise points
noise points
diagnostics
diagnostics
create binary histogram
create binary hist...
estimate visibility
estimate visibility
output pointcloud (PointXYZIRADT)
output pointcloud...
set ROI
set ROIText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp index 6e46123cfba4e..41c2130058f39 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp @@ -30,6 +30,8 @@ #include #include +#include +#include #include namespace pointcloud_preprocessor @@ -48,6 +50,12 @@ enum ReturnType : uint8_t { DUAL_ONLY, }; +std::unordered_map roi_mode_map_ = { + {"No_ROI", 0}, + {"Fixed_xyz_ROI", 1}, + {"Fixed_azimuth_ROI", 2}, +}; + class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter { protected: @@ -66,13 +74,25 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter private: void onVisibilityChecker(DiagnosticStatusWrapper & stat); Updater updater_{this}; - double visibility_ = 1.f; + double visibility_ = -1.0f; double weak_first_distance_ratio_; double general_distance_ratio_; int weak_first_local_noise_threshold_; - double visibility_threshold_; + double visibility_error_threshold_; + double visibility_warn_threshold_; int vertical_bins_; float max_azimuth_diff_; + std::string roi_mode_; + float x_max_; + float x_min_; + float y_max_; + float y_min_; + float z_max_; + float z_min_; + + float min_azimuth_deg_; + float max_azimuth_deg_; + float max_distance_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index 7858322713063..56d26e420464d 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -34,6 +34,15 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( { // set initial parameters { + x_max_ = static_cast(declare_parameter("x_max", 18.0)); + x_min_ = static_cast(declare_parameter("x_min", -12.0)); + y_max_ = static_cast(declare_parameter("y_max", 2.0)); + y_min_ = static_cast(declare_parameter("y_min", -2.0)); + z_max_ = static_cast(declare_parameter("z_max", 10.0)); + z_min_ = static_cast(declare_parameter("z_min", 0.0)); + min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 135.0)); + max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 225.0)); + max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); max_azimuth_diff_ = static_cast(declare_parameter("max_azimuth_diff", 50.0)); weak_first_distance_ratio_ = @@ -42,8 +51,12 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( static_cast(declare_parameter("general_distance_ratio", 1.03)); weak_first_local_noise_threshold_ = - static_cast(declare_parameter("weak_first_local_noise_threshold", 10)); - visibility_threshold_ = static_cast(declare_parameter("visibility_threshold", 0.5)); + static_cast(declare_parameter("weak_first_local_noise_threshold", 2)); + roi_mode_ = static_cast(declare_parameter("roi_mode", "Fixed_xyz_ROI")); + visibility_error_threshold_ = + static_cast(declare_parameter("visibility_error_threshold", 0.5)); + visibility_warn_threshold_ = + static_cast(declare_parameter("visibility_warn_threshold", 0.7)); } updater_.setHardwareID("dual_return_outlier_filter"); updater_.add( @@ -52,11 +65,11 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( updater_.setPeriod(0.1); image_pub_ = - image_transport::create_publisher(this, "/dual_return_outlier_filter/frequency_image"); + image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image"); visibility_pub_ = create_publisher( - "/dual_return_outlier_filter/visibility", rclcpp::SensorDataQoS()); + "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); noise_cloud_pub_ = create_publisher( - "/dual_return_outlier_filter/pointcloud_noise", rclcpp::SensorDataQoS()); + "dual_return_outlier_filter/debug/pointcloud_noise", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( @@ -69,15 +82,27 @@ void DualReturnOutlierFilterComponent::onVisibilityChecker(DiagnosticStatusWrapp stat.add("value", std::to_string(visibility_)); // Judge level - const auto level = - visibility_ > visibility_threshold_ ? DiagnosticStatus::OK : DiagnosticStatus::WARN; + auto level = DiagnosticStatus::OK; + if (visibility_ < 0) { + level = DiagnosticStatus::STALE; + } else if (visibility_ < visibility_error_threshold_) { + level = DiagnosticStatus::ERROR; + } else if (visibility_ < visibility_warn_threshold_) { + level = DiagnosticStatus::WARN; + } else { + level = DiagnosticStatus::OK; + } // Set message std::string msg; if (level == DiagnosticStatus::OK) { msg = "OK"; } else if (level == DiagnosticStatus::WARN) { - msg = "low visibility in dual outlier filter"; + msg = "WARNING: low visibility in dual outlier filter"; + } else if (level == DiagnosticStatus::ERROR) { + msg = "ERROR: low visibility in dual outlier filter"; + } else if (level == DiagnosticStatus::STALE) { + msg = "STALE"; } stat.summary(level, msg); } @@ -93,6 +118,23 @@ void DualReturnOutlierFilterComponent::filter( uint32_t vertical_bins = vertical_bins_; uint32_t horizontal_bins = 36; + float max_azimuth = 36000.0f; + float min_azimuth = 0.0f; + switch (roi_mode_map_[roi_mode_]) { + case 2: { + max_azimuth = max_azimuth_deg_ * 100.0; + min_azimuth = min_azimuth_deg_ * 100.0; + break; + } + + default: { + max_azimuth = 36000.0f; + min_azimuth = 0.0f; + break; + } + } + + uint32_t horizontal_res = static_cast((max_azimuth - min_azimuth) / horizontal_bins); pcl::PointCloud::Ptr pcl_output( new pcl::PointCloud); @@ -124,7 +166,7 @@ void DualReturnOutlierFilterComponent::filter( if (weak_first_single_ring.points.size() < 2) { continue; } - std::vector deleted_azimuths; + std::vector deleted_azimuths; std::vector deleted_distances; pcl::PointCloud temp_segment; @@ -146,9 +188,35 @@ void DualReturnOutlierFilterComponent::filter( // Analyse segment points here } else { // Log the deleted azimuth and its distance for analysis - deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); - deleted_distances.push_back(iter->distance); - noise_output->points.push_back(*iter); + switch (roi_mode_map_[roi_mode_]) { + case 1: // base_link xyz-ROI + { + if ( + iter->x > x_min_ && iter->x < x_max_ && iter->y > y_min_ && iter->y < y_max_ && + iter->z > z_min_ && iter->z < z_max_) { + deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + deleted_distances.push_back(iter->distance); + noise_output->points.push_back(*iter); + } + break; + } + case 2: { + if ( + iter->azimuth > min_azimuth && iter->azimuth < max_azimuth && + iter->distance < max_distance_) { + deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + noise_output->points.push_back(*iter); + deleted_distances.push_back(iter->distance); + } + break; + } + default: { + deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + deleted_distances.push_back(iter->distance); + noise_output->points.push_back(*iter); + break; + } + } } } // Analyse last segment points here @@ -160,27 +228,54 @@ void DualReturnOutlierFilterComponent::filter( continue; } while ((uint)deleted_azimuths[current_deleted_index] < - ((i + 1) * (36000 / horizontal_bins)) && + ((i + static_cast(min_azimuth / horizontal_res) + 1) * horizontal_res) && current_deleted_index < (deleted_azimuths.size() - 1)) { noise_frequency[i] = noise_frequency[i] + 1; current_deleted_index++; } - if (temp_segment.points.size() == 0) { - continue; - } - while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f - ? 0.f - : temp_segment.points[current_temp_segment_index].azimuth) < - ((i + 1) * (36000 / horizontal_bins)) && - current_temp_segment_index < (temp_segment.points.size() - 1)) { - if (noise_frequency[i] < weak_first_local_noise_threshold_) { - pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]); - } else { - noise_frequency[i] = noise_frequency[i] + 1; - noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + if (temp_segment.points.size() > 0) { + while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f + ? 0.f + : temp_segment.points[current_temp_segment_index].azimuth) < + ((i + 1 + static_cast(min_azimuth / horizontal_res)) * horizontal_res) && + current_temp_segment_index < (temp_segment.points.size() - 1)) { + if (noise_frequency[i] < weak_first_local_noise_threshold_) { + pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } else { + switch (roi_mode_map_[roi_mode_]) { + case 1: { + if ( + temp_segment.points[current_temp_segment_index].x < x_max_ && + temp_segment.points[current_temp_segment_index].x > x_min_ && + temp_segment.points[current_temp_segment_index].y > y_max_ && + temp_segment.points[current_temp_segment_index].y < y_min_ && + temp_segment.points[current_temp_segment_index].z < z_max_ && + temp_segment.points[current_temp_segment_index].z > z_min_) { + noise_frequency[i] = noise_frequency[i] + 1; + noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } + break; + } + case 2: { + if ( + temp_segment.points[current_temp_segment_index].azimuth < max_azimuth && + temp_segment.points[current_temp_segment_index].azimuth > min_azimuth && + temp_segment.points[current_temp_segment_index].distance < max_distance_) { + noise_frequency[i] = noise_frequency[i] + 1; + noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } + break; + } + default: { + noise_frequency[i] = noise_frequency[i] + 1; + noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + break; + } + } + } + current_temp_segment_index++; + frequency_image.at(ring_id, i) = noise_frequency[i]; } - current_temp_segment_index++; - frequency_image.at(ring_id, i) = noise_frequency[i]; } } } @@ -217,6 +312,7 @@ void DualReturnOutlierFilterComponent::filter( pcl_output->points.push_back(tmp_p); } } + // Threshold for diagnostics (tunable) cv::Mat binary_image; cv::inRange(frequency_image, weak_first_local_noise_threshold_, 255, binary_image); @@ -230,7 +326,7 @@ void DualReturnOutlierFilterComponent::filter( cv::applyColorMap(frequency_image * 4, frequency_image_colorized, cv::COLORMAP_JET); sensor_msgs::msg::Image::SharedPtr frequency_image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frequency_image_colorized).toImageMsg(); - + frequency_image_msg->header = input->header; // Publish histogram image image_pub_.publish(frequency_image_msg); tier4_debug_msgs::msg::Float32Stamped visibility_msg; diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 12faac2799b13..4818bb554c5a1 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -5,7 +5,6 @@ - @@ -18,7 +17,7 @@ - + @@ -51,48 +50,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index a14d083a4e2e4..da5939bccea7d 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -180,9 +180,7 @@ void DummyPerceptionPublisherNode::timerCallback() new pcl::PointCloud); pcl::VoxelGridOcclusionEstimation ray_tracing_filter; ray_tracing_filter.setInputCloud(merged_pointcloud_ptr); - // above this value raytrace won't be as expected - const double leaf_size = 0.02; - ray_tracing_filter.setLeafSize(leaf_size, leaf_size, leaf_size); + ray_tracing_filter.setLeafSize(0.25, 0.25, 0.25); ray_tracing_filter.initializeVoxelGrid(); for (size_t i = 0; i < v_pointcloud.size(); ++i) { pcl::PointCloud::Ptr ray_traced_pointcloud_ptr( diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml index 03610e09f1f45..bfbb6beeaf054 100644 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml +++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml @@ -1,4 +1,4 @@ -# Autoware State Monitor Parameters +# AD Service State Monitor Parameters /**: ros__parameters: # modules_names: string array diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml index 41928923a0dfd..a3b20c4c94d31 100644 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml +++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml @@ -1,4 +1,4 @@ -# Autoware State Monitor: Planning Simulator Parameters +# AD Service State Monitor: Planning Simulator Parameters /**: ros__parameters: # modules_names: string array diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index b863a19547b39..719fdcf0b1d17 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -268,9 +268,9 @@ void HDDMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) stat.add(fmt::format("HDD {}: status", hdd_index), usage_dict_.at(level)); stat.add(fmt::format("HDD {}: filesystem", hdd_index), list[0].c_str()); - stat.add(fmt::format("HDD {}: size (MB)", hdd_index), list[1].c_str()); - stat.add(fmt::format("HDD {}: used (MB)", hdd_index), list[2].c_str()); - stat.add(fmt::format("HDD {}: avail (MB)", hdd_index), list[3].c_str()); + stat.add(fmt::format("HDD {}: size", hdd_index), (list[1] + " MiB").c_str()); + stat.add(fmt::format("HDD {}: used", hdd_index), (list[2] + " MiB").c_str()); + stat.add(fmt::format("HDD {}: avail", hdd_index), (list[3] + " MiB").c_str()); stat.add(fmt::format("HDD {}: use", hdd_index), list[4].c_str()); std::string mounted_ = list[5]; if (list.size() > 6) {