diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index d6048f59d473f..a0b687f89ca40 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,12 +1,11 @@ *:*/test/* -arrayIndexThenCheck checkersReport -constParameterPointer constParameterReference constVariable constVariableReference containerOutOfBounds +// cspell: ignore cstyle cstyleCast duplicateBranch duplicateBreak @@ -23,11 +22,10 @@ noValidConfiguration passedByValue preprocessorErrorDirective redundantInitialization -returnByReference -shadowArgument shadowFunction shadowVariable syntaxError +// cspell: ignore uninit uninitMemberVar unknownMacro unmatchedSuppression diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index ade750db259b9..d47a999ad9254 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -185,12 +185,12 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_mod planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md deleted file mode 100644 index 97b0e95452426..0000000000000 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ /dev/null @@ -1,8 +0,0 @@ -**Note**: Confirm the [contribution guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/) before submitting a pull request. - -Click the `Preview` tab and select a PR template: - -- [Standard change](?expand=1&template=standard-change.md) -- [Small change](?expand=1&template=small-change.md) - -**Do NOT send a PR with this description.** diff --git a/.github/PULL_REQUEST_TEMPLATE/small-change.md b/.github/PULL_REQUEST_TEMPLATE/small-change.md deleted file mode 100644 index e15fdd992d114..0000000000000 --- a/.github/PULL_REQUEST_TEMPLATE/small-change.md +++ /dev/null @@ -1,44 +0,0 @@ -## Description - - - -## Tests performed - - - - -Not applicable. - -## Effects on system behavior - - - -Not applicable. - -## Interface changes - - - -## Pre-review checklist for the PR author - -The PR author **must** check the checkboxes below when creating the PR. - -- [ ] I've confirmed the [contribution guidelines]. -- [ ] The PR follows the [pull request guidelines]. - -## In-review checklist for the PR reviewers - -The PR reviewers **must** check the checkboxes below before approval. - -- [ ] The PR follows the [pull request guidelines]. - -## Post-review checklist for the PR author - -The PR author **must** check the checkboxes below before merging. - -- [ ] There are no open discussions or they are tracked via tickets. - -After all checkboxes are checked, anyone who has write access can merge the PR. - -[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/ -[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/ diff --git a/.github/PULL_REQUEST_TEMPLATE/standard-change.md b/.github/PULL_REQUEST_TEMPLATE/standard-change.md deleted file mode 100644 index 391af629751e0..0000000000000 --- a/.github/PULL_REQUEST_TEMPLATE/standard-change.md +++ /dev/null @@ -1,63 +0,0 @@ -## Description - - - -## Related links - - - -## Tests performed - - - -## Notes for reviewers - - - -## Interface changes - - - -### ROS Topic Changes - - - - - - -### ROS Parameter Changes - - - - - -## Effects on system behavior - - - -## Pre-review checklist for the PR author - -The PR author **must** check the checkboxes below when creating the PR. - -- [ ] I've confirmed the [contribution guidelines]. -- [ ] The PR follows the [pull request guidelines]. - -## In-review checklist for the PR reviewers - -The PR reviewers **must** check the checkboxes below before approval. - -- [ ] The PR follows the [pull request guidelines]. -- [ ] The PR has been properly tested. -- [ ] The PR has been reviewed by the code owners. - -## Post-review checklist for the PR author - -The PR author **must** check the checkboxes below before merging. - -- [ ] There are no open discussions or they are tracked via tickets. -- [ ] The PR is ready for merge. - -After all checkboxes are checked, anyone who has write access can merge the PR. - -[contribution guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/ -[pull request guidelines]: https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/ diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000000000..fcaf3601fef92 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,61 @@ +## Description + +## Related links + +**Parent Issue:** + +- Link + + + +## How was this PR tested? + +## Notes for reviewers + +None. + +## Interface changes + +None. + + + +## Effects on system behavior + +None. diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 263df86438787..151890e2783d9 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -7,9 +7,6 @@ - source: .github/ISSUE_TEMPLATE/bug.yaml - source: .github/ISSUE_TEMPLATE/config.yml - source: .github/ISSUE_TEMPLATE/task.yaml - - source: .github/PULL_REQUEST_TEMPLATE.md - - source: .github/PULL_REQUEST_TEMPLATE/small-change.md - - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md - source: .github/dependabot.yaml - source: .github/stale.yml - source: .github/workflows/cancel-previous-workflows.yaml diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml index a59287a6d9b37..60337cc572e7a 100644 --- a/.github/workflows/clang-tidy-differential.yaml +++ b/.github/workflows/clang-tidy-differential.yaml @@ -20,14 +20,10 @@ jobs: runs-on: ubuntu-latest container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" - - name: Checkout PR branch and all PR commits uses: actions/checkout@v4 with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} + fetch-depth: 0 - name: Show disk space before the tasks run: df -h @@ -41,19 +37,17 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v42 - with: - files: | - **/*.cpp - **/*.hpp + run: | + echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | tr '\n' ' ' || true)" >> $GITHUB_OUTPUT + shell: bash - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} + if: ${{ steps.get-modified-files.outputs.changed_files != '' }} uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 with: rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} + target-files: ${{ steps.get-modified-files.outputs.changed_files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos diff --git a/.github/workflows/comment-on-pr.yaml b/.github/workflows/comment-on-pr.yaml new file mode 100644 index 0000000000000..e517016a300d9 --- /dev/null +++ b/.github/workflows/comment-on-pr.yaml @@ -0,0 +1,25 @@ +name: comment-on-pr +on: + pull_request_target: + +jobs: + comment-on-pr: + runs-on: ubuntu-latest + permissions: + pull-requests: write + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Initial PR comment + uses: marocchino/sticky-pull-request-comment@v2 + with: + message: | + Thank you for contributing to the Autoware project! + + 🚧 If your pull request is in progress, [switch it to draft mode](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/changing-the-stage-of-a-pull-request#converting-a-pull-request-to-a-draft). + + Please ensure: + - You've checked our [contribution guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/). + - Your PR follows our [pull request guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/pull-request-guidelines/). + - All required CI checks pass before [marking the PR ready for review](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/changing-the-stage-of-a-pull-request#marking-a-pull-request-as-ready-for-review). diff --git a/.github/workflows/cppcheck-daily.yaml b/.github/workflows/cppcheck-daily.yaml index df845b2154929..03ea75105b360 100644 --- a/.github/workflows/cppcheck-daily.yaml +++ b/.github/workflows/cppcheck-daily.yaml @@ -22,7 +22,7 @@ jobs: continue-on-error: true id: cppcheck run: | - cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + cppcheck --enable=all --inconclusive --check-level=exhaustive --suppress=*:*/test/* --error-exitcode=1 --xml . 2> cppcheck-report.xml shell: bash - name: Count errors by error ID and severity diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml index 99f8aa1311b5e..7fe37ea006596 100644 --- a/.github/workflows/cppcheck-differential.yaml +++ b/.github/workflows/cppcheck-differential.yaml @@ -31,37 +31,33 @@ jobs: run: git fetch origin ${{ github.base_ref }} shell: bash - - name: Get changed files - id: changed-files + - name: Get changed files (existing files only) + id: get-changed-files run: | - git diff --name-only "origin/${{ github.base_ref }}"...HEAD > changed_files.txt - cat changed_files.txt + echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT shell: bash - name: Run Cppcheck on changed files + if: ${{ steps.get-changed-files.outputs.changed-files != '' }} continue-on-error: true id: cppcheck run: | - files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true) - if [ -n "$files" ]; then - echo "Running Cppcheck on changed files: $files" - cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt - else - echo "No C++ files changed." - touch cppcheck-report.txt - fi + echo "Running Cppcheck on changed files: ${{ steps.get-changed-files.outputs.changed-files }}" + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions ${{ steps.get-changed-files.outputs.changed-files }} 2> cppcheck-report.txt shell: bash - name: Show cppcheck-report result + if: ${{ steps.get-changed-files.outputs.changed-files != '' }} run: | cat cppcheck-report.txt - name: Upload Cppcheck report + if: ${{ steps.get-changed-files.outputs.changed-files != '' }} uses: actions/upload-artifact@v2 with: name: cppcheck-report path: cppcheck-report.txt - name: Fail the job if Cppcheck failed - if: steps.cppcheck.outcome == 'failure' + if: ${{ steps.get-changed-files.outputs.changed-files != '' && steps.cppcheck.outcome == 'failure' }} run: exit 1 diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index d7de1397bfaac..254a2d03f71d1 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -527,6 +528,7 @@ class AutowareTestManager AutowareTestManager() { test_node_ = std::make_shared("autoware_test_manager_node"); + pub_clock_ = test_node_->create_publisher("/clock", 1); } template @@ -560,10 +562,28 @@ class AutowareTestManager } } + /** + * @brief Publishes a ROS Clock message with the specified time. + * + * This function publishes a ROS Clock message with the specified time. + * Be careful when using this function, as it can affect the behavior of + * the system under test. Consider using ament_add_ros_isolated_gtest to + * isolate the system under test from the ROS clock. + * + * @param time The time to publish. + */ + void jump_clock(const rclcpp::Time & time) + { + rosgraph_msgs::msg::Clock clock; + clock.clock = time; + pub_clock_->publish(clock); + } + protected: // Publisher std::unordered_map> publishers_; std::unordered_map> subscribers_; + rclcpp::Publisher::SharedPtr pub_clock_; // Node rclcpp::Node::SharedPtr test_node_; diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index 563350dbe53cc..d870bcf9974a1 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include namespace autoware::test_utils @@ -54,14 +56,32 @@ lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; + if (!errors.empty()) { + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; } - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + for (lanelet::Point3d point : map->pointLayer) { + if (point.hasAttribute("local_x")) { + point.x() = point.attribute("local_x").asDouble().value(); + } + if (point.hasAttribute("local_y")) { + point.y() = point.attribute("local_y").asDouble().value(); + } } - return nullptr; + + // realign lanelet borders using updated points + for (lanelet::Lanelet lanelet : map->laneletLayer) { + auto left = lanelet.leftBound(); + auto right = lanelet.rightBound(); + std::tie(left, right) = lanelet::geometry::align(left, right); + lanelet.setLeftBound(left); + lanelet.setRightBound(right); + } + + return map; } LaneletMapBin convertToMapBinMsg( diff --git a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp index aabaea7ca6339..6691c1fb9e97d 100644 --- a/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp +++ b/common/tensorrt_common/include/tensorrt_common/tensorrt_common.hpp @@ -15,7 +15,9 @@ #ifndef TENSORRT_COMMON__TENSORRT_COMMON_HPP_ #define TENSORRT_COMMON__TENSORRT_COMMON_HPP_ +#ifndef YOLOX_STANDALONE #include +#endif #include #include @@ -86,6 +88,7 @@ struct BuildConfig profile_per_layer(profile_per_layer), clip_value(clip_value) { +#ifndef YOLOX_STANDALONE if ( std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == valid_calib_type.end()) { @@ -95,6 +98,7 @@ struct BuildConfig << "Default calibration type will be used: MinMax" << std::endl; std::cerr << message.str(); } +#endif } }; diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp index bd5c3349d3c35..16c3f4b5def96 100644 --- a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include namespace tier4_camera_view_rviz_plugin diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp index c25787af2f47b..8ca2d27388413 100644 --- a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp @@ -46,7 +46,7 @@ #include "rviz_common/frame_position_tracking_view_controller.hpp" #include -#include +#include namespace rviz_common { diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp index 9f4306a9d982a..ed47a15270c3a 100644 --- a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp index 7f2cf6a8d17ee..d80fa4502a48f 100644 --- a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp @@ -45,7 +45,7 @@ #include "rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp" -#include +#include #include diff --git a/common/tier4_perception_rviz_plugin/src/tools/util.hpp b/common/tier4_perception_rviz_plugin/src/tools/util.hpp index 2126af001aeb5..bf951aad5006a 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/util.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/util.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp index 96345205289e6..cb191811ddbc3 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index e6421b2af84bb..876797a58df25 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Publisher autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + this, "~/debug/processing_time_ms_diag"}; + rclcpp::Publisher::SharedPtr processing_time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 427cf0898470e..dde37b03ffecc 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o lane_departure_checker_->setParam(param_, vehicle_info); // Publisher + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Nothing // Diagnostic Updater @@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer() } processing_time_map["Total"] = stop_watch.toc("Total"); - processing_time_publisher_.publish(processing_time_map); + processing_diag_publisher_.publish(processing_time_map); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = processing_time_map["Total"]; + processing_time_publisher_->publish(processing_time_msg); } rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 058eb45bfaaff..902790f260e5e 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -271,7 +271,7 @@ class MPC */ std::pair executeOptimization( const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt, - const MPCTrajectory & trajectory, const double current_velocity); + const MPCTrajectory & trajectory); /** * @brief Resample the trajectory with the MPC resampling time. @@ -386,8 +386,7 @@ class MPC * @param reference_trajectory The reference trajectory. * @param current_velocity current velocity of ego. */ - VectorXd calcSteerRateLimitOnTrajectory( - const MPCTrajectory & trajectory, const double current_velocity) const; + VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const; //!< @brief logging with warn and return false template diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index c8e2da6daf7f4..ea97e9e6d5f39 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -76,9 +76,8 @@ bool MPC::calculateMPC( const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt); // solve Optimization problem - const auto [success_opt, Uex] = executeOptimization( - mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory, - current_kinematics.twist.twist.linear.x); + const auto [success_opt, Uex] = + executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory); if (!success_opt) { return fail_warn_throttle("optimization failed. Stop MPC."); } @@ -544,8 +543,7 @@ MPCMatrix MPC::generateMPCMatrix( * [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U) */ std::pair MPC::executeOptimization( - const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj, - const double current_velocity) + const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj) { VectorXd Uex; @@ -578,7 +576,7 @@ std::pair MPC::executeOptimization( VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle // steering angle rate limit - VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj); VectorXd ubA = steer_rate_limits * prediction_dt; VectorXd lbA = -steer_rate_limits * prediction_dt; ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; @@ -730,8 +728,7 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } -VectorXd MPC::calcSteerRateLimitOnTrajectory( - const MPCTrajectory & trajectory, const double current_velocity) const +VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const { const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { std::vector reference, limits; @@ -765,13 +762,6 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( return reference.back(); }; - // when the vehicle is stopped, no steering rate limit. - constexpr double steer_rate_lim = 5.0; - const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; - if (is_vehicle_stopped) { - return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon); - } - // calculate steering rate limit VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); for (int i = 0; i < m_param.prediction_horizon; ++i) { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 9291d538b022f..5805ef7db9f86 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -64,7 +64,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro { public: /// \param node Reference to the node used only for the component and parameter initialization. - explicit PidLongitudinalController(rclcpp::Node & node); + explicit PidLongitudinalController( + rclcpp::Node & node, std::shared_ptr diag_updater); private: struct Motion @@ -236,8 +237,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic - - diagnostic_updater::Updater diagnostic_updater_; + std::shared_ptr + diag_updater_{}; // Diagnostic updater for publishing diagnostic data. struct DiagnosticData { double trans_deviation{0.0}; // translation deviation between nearest point and current_pose diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 78c7ccf832514..93496c85c741b 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -27,14 +27,16 @@ namespace autoware::motion::control::pid_longitudinal_controller { -PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) +PidLongitudinalController::PidLongitudinalController( + rclcpp::Node & node, std::shared_ptr diag_updater) : node_parameters_(node.get_node_parameters_interface()), clock_(node.get_clock()), - logger_(node.get_logger().get_child("longitudinal_controller")), - diagnostic_updater_(&node) + logger_(node.get_logger().get_child("longitudinal_controller")) { using std::placeholders::_1; + diag_updater_ = diag_updater; + // parameters timer m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); @@ -432,7 +434,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( publishDebugData(ctrl_cmd, control_data); // diagnostic - diagnostic_updater_.force_update(); + diag_updater_->force_update(); return output; } @@ -1150,8 +1152,8 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da void PidLongitudinalController::setupDiagnosticUpdater() { - diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller"); - diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState); + diag_updater_->setHardwareID("autoware_pid_longitudinal_controller"); + diag_updater_->add("control_state", this, &PidLongitudinalController::checkControlState); } void PidLongitudinalController::checkControlState( diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py index fb96e134f7746..bd6cf28c0b629 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py @@ -297,7 +297,7 @@ for curDir, dirs, files in os.walk(control_dir_path): for name in files: if name == "vehicle_cmd_gate.param.yaml": - if curDir.split("/")[-2] == "vehicle_cmd_gate": + if curDir.split("/")[-2] == "autoware_vehicle_cmd_gate": limit_yaml_path = curDir + "/" + name break diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 78a120fe601f2..196657fab6e91 100755 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -16,6 +16,7 @@ # cspell: ignore interp +from enum import Enum import time from autoware_adapi_v1_msgs.msg import OperationModeState @@ -26,6 +27,9 @@ from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration +from diagnostic_msgs.msg import DiagnosticStatus +import diagnostic_updater +from diagnostic_updater import DiagnosticStatusWrapper from geometry_msgs.msg import AccelWithCovarianceStamped from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Odometry @@ -42,6 +46,13 @@ from tier4_debug_msgs.msg import Float32Stamped +class ControlStatus(Enum): + DRIVE = 0 + STOPPING = 1 + STOPPED = 2 + EMERGENCY = 3 + + def getYaw(orientation_xyzw): return R.from_quat(orientation_xyzw.reshape(-1, 4)).as_euler("xyz")[:, 2] @@ -272,6 +283,10 @@ def __init__(self): self.last_steer_cmd = 0.0 self.past_control_trajectory_mode = 1 + self.diagnostic_updater = diagnostic_updater.Updater(self) + self.setup_diagnostic_updater() + self.control_state = ControlStatus.STOPPED + def onTrajectory(self, msg): self._present_trajectory = msg @@ -421,12 +436,14 @@ def control(self): orientation_deviation_threshold = 60 * np.pi / 180.0 # The moment the threshold is exceeded, it enters emergency stop mode. + control_state = self.control_state if is_applying_control: if ( position_deviation > position_deviation_threshold or orientation_deviation > orientation_deviation_threshold ): self.emergency_stop_mode_flag = True + control_state = ControlStatus.EMERGENCY # Normal return from emergency stop mode when within the threshold value and a request to cancel the stop mode has been received. if ( @@ -698,12 +715,13 @@ def control(self): steer_cmd = self.last_steer_cmd + steer_cmd_decrease_limit else: steer_cmd = 0.0 + control_state = ControlStatus.STOPPING cmd_msg = Control() cmd_msg.stamp = cmd_msg.lateral.stamp = cmd_msg.longitudinal.stamp = ( self.get_clock().now().to_msg() ) - cmd_msg.longitudinal.speed = trajectory_longitudinal_velocity[nearestIndex] + cmd_msg.longitudinal.velocity = trajectory_longitudinal_velocity[nearestIndex] cmd_msg.longitudinal.acceleration = acc_cmd cmd_msg.lateral.steering_tire_angle = steer_cmd @@ -722,6 +740,15 @@ def control(self): self.control_cmd_steer_list.pop(0) self.control_cmd_acc_list.pop(0) + # [3-6] Update control state + if control_state != ControlStatus.EMERGENCY: + stopped_velocity_threshold = 0.1 + if present_linear_velocity[0] <= stopped_velocity_threshold: + control_state = ControlStatus.STOPPED + elif control_state != ControlStatus.STOPPING: + control_state = ControlStatus.DRIVE + self.control_state = control_state + # [4] Update MPC internal variables if not is_applying_control: self.controller.send_initialize_input_queue() @@ -843,6 +870,23 @@ def control(self): ) ) + self.diagnostic_updater.force_update() + + def setup_diagnostic_updater(self): + self.diagnostic_updater.setHardwareID("pympc_trajectory_follower") + self.diagnostic_updater.add("control_state", self.check_control_state) + + def check_control_state(self, stat: DiagnosticStatusWrapper): + msg = "emergency occurred" if self.control_state == ControlStatus.EMERGENCY else "OK" + level = ( + DiagnosticStatus.ERROR + if self.control_state == ControlStatus.EMERGENCY + else DiagnosticStatus.OK + ) + stat.summary(level, msg) + stat.add("control_state", str(self.control_state)) + return stat + def main(args=None): rclpy.init(args=args) diff --git a/control/autoware_smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py index fee1d04e826c0..7df489ff29377 100644 --- a/control/autoware_smart_mpc_trajectory_follower/setup.py +++ b/control/autoware_smart_mpc_trajectory_follower/setup.py @@ -28,7 +28,7 @@ ] ) setup( - name="smart_mpc_trajectory_follower", + name="autoware_smart_mpc_trajectory_follower", version="1.0.0", packages=find_packages(), package_data={ diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index c1e5fe646cdaa..d5167f5096786 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -57,7 +57,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control switch (longitudinal_controller_mode) { case LongitudinalControllerMode::PID: { longitudinal_controller_ = - std::make_shared(*this); + std::make_shared( + *this, diag_updater_); break; } default: diff --git a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 46130fc8f9941..66dde51780ffa 100644 --- a/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -294,7 +294,12 @@ class PubSubNode : public rclcpp::Node const auto max_lat_acc_lim = *std::max_element(lat_acc_lim.begin(), lat_acc_lim.end()); const auto max_lat_jerk_lim = *std::max_element(lat_jerk_lim.begin(), lat_jerk_lim.end()); + // This test is designed to verify that the filter is applied correctly. However, if topic + // communication is delayed, the allowable range of change in the command values between the + // sender and receiver of the topic will vary, making the results dependent on processing time. + // We define the allowable error margin to account for this. constexpr auto threshold_scale = 1.1; + // Output command must be smaller than maximum limit. // TODO(Horibe): check for each velocity range. if (std::abs(lon_vel) > 0.01) { @@ -433,7 +438,7 @@ TEST_P(TestFixture, CheckFilterForSinCmd) // << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias // << ")" << std::endl; - for (size_t i = 0; i < 100; ++i) { + for (size_t i = 0; i < 30; ++i) { auto start_time = std::chrono::steady_clock::now(); const bool reset_clock = (i == 0); @@ -449,7 +454,13 @@ TEST_P(TestFixture, CheckFilterForSinCmd) std::chrono::milliseconds elapsed = std::chrono::duration_cast(end_time - start_time); - std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{10} - elapsed; + // The value determines the period of the topic. The filter logic of vehicle_cmd_gate depends on + // real-time, and if the time from publishing to subscribing becomes too long, this test will + // fail (the test specification itself should be improved). To prevent processing bottlenecks, + // please set this value appropriately. It is set to 30ms because it occasionally fails at 10ms. + constexpr int running_ms = 30; + + std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{running_ms} - elapsed; if (sleep_duration.count() > 0) { std::this_thread::sleep_for(sleep_duration); } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 1cf3cb5b44656..15cab8df52ede 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -444,7 +444,7 @@ def launch_setup(context, *args, **kwargs): smart_mpc_trajectory_follower = Node( package="autoware_smart_mpc_trajectory_follower", executable="pympc_trajectory_follower.py", - name="pympc_trajectory_follower", + name="controller_node_exe", ) if trajectory_follower_mode == "trajectory_follower_node": return [group, control_validator_group] diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 228b4c89f95d3..373e343ad64fa 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,5 +1,8 @@ + + + @@ -10,15 +13,13 @@ + - + + + - - - - - @@ -43,10 +44,21 @@ - - + + + + + + + + + + + + + @@ -56,35 +68,58 @@ + + + + + - + + + + + + + + - + + + + + + + + - + + + - + + - + + @@ -118,11 +153,12 @@ - + + + - @@ -133,6 +169,7 @@ + @@ -141,9 +178,9 @@ - + - + @@ -152,7 +189,7 @@ - + @@ -175,10 +212,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -208,8 +273,14 @@ - + + + + + + + @@ -250,8 +321,12 @@ - + + + + + @@ -264,32 +339,41 @@ - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - - + + + + + + + - + - + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index 3c8480038f320..4c288aa632182 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -1,17 +1,11 @@ - - - - - - - + @@ -38,6 +32,14 @@ + + + + + + + + - - - - - - - - - - - @@ -230,7 +220,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index ab34cc49dd2f6..98e8ec7c8ed4b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -1,18 +1,19 @@ - - - - + + + + + @@ -21,7 +22,7 @@ - + @@ -41,7 +42,7 @@ - + @@ -77,7 +78,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index 819a6337138a1..38ee946201f7e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -1,22 +1,10 @@ - + + - - - - - - - - - - - - - - + @@ -43,7 +31,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml index 0efe7444fcba5..b85d4b02847b3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml @@ -1,10 +1,12 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml index 85b404ba89b1f..d4a9e40f29d97 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -44,9 +44,9 @@ - - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index 16ec8f8fbd573..6ea745a8bae2c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -47,16 +47,16 @@ - - - - - + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index b8172daa616fa..1975ad2d15eb9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -1,5 +1,8 @@ + + + @@ -11,52 +14,87 @@ + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 8f0513bcd5446..5d8d7563e7a7c 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -124,6 +124,9 @@ /> + + + @@ -189,6 +192,7 @@ + @@ -255,7 +259,9 @@ + + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 97b07410d108a..7dc33e0e82ea9 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -78,6 +78,7 @@ + diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md index 37596ee9820b9..14501c689dbe6 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md @@ -46,7 +46,7 @@ ros2 launch autoware_launch ... \ ### Rosbag -#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) +#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1ZPsfDvOXFrMxtx7fb1W5sOXdAK1e71hY/view) This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. @@ -55,7 +55,7 @@ It is a known problem that the timing of when each AR tag begins to be detected ![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) -#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) +#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1VQCQ_qiEZpCMI3-z6SNs__zJ-4HJFQjx/view) Please remap the topic names and play it. diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index c39969fb3cc5f..cef3debf22a4f 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -51,7 +51,11 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif #include #include diff --git a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index a178a305f6d12..0f293e4d31cac 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -15,6 +15,8 @@ #ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ #define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ +#include "localization_util/covariance_ellipse.hpp" + #include #include #include @@ -25,15 +27,6 @@ #include -struct Ellipse -{ - double long_radius; - double short_radius; - double yaw; - Eigen::Matrix2d P; - double size_lateral_direction; -}; - class LocalizationErrorMonitor : public rclcpp::Node { private: @@ -50,12 +43,9 @@ class LocalizationErrorMonitor : public rclcpp::Node double warn_ellipse_size_; double error_ellipse_size_lateral_direction_; double warn_ellipse_size_lateral_direction_; - Ellipse ellipse_; + autoware::localization_util::Ellipse ellipse_; void on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); - visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); - static double measure_size_ellipse_along_body_frame(const Eigen::Matrix2d & Pinv, double theta); public: explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 681da7e57c6c5..b8a466f95dfab 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -23,6 +23,7 @@ autoware_universe_utils diagnostic_msgs diagnostic_updater + localization_util nav_msgs rclcpp_components tf2 diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp index 204afa7bdec2f..44223b0fd1670 100644 --- a/localization/localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -58,61 +58,12 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); } -visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker( - const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom) -{ - tf2::Quaternion quat; - quat.setEuler(0, 0, ellipse.yaw); - - const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); - const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); - visualization_msgs::msg::Marker marker; - marker.header = odom->header; - marker.header.stamp = this->now(); - marker.ns = "error_ellipse"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = odom->pose.pose; - marker.pose.orientation = tf2::toMsg(quat); - marker.scale.x = ellipse_long_radius * 2; - marker.scale.y = ellipse_short_radius * 2; - marker.scale.z = 0.01; - marker.color.a = 0.1; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - return marker; -} - void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) { - // create xy covariance (2x2 matrix) - // input geometry_msgs::PoseWithCovariance contain 6x6 matrix - Eigen::Matrix2d xy_covariance; - const auto cov = input_msg->pose.covariance; - xy_covariance(0, 0) = cov[0 * 6 + 0]; - xy_covariance(0, 1) = cov[0 * 6 + 1]; - xy_covariance(1, 0) = cov[1 * 6 + 0]; - xy_covariance(1, 1) = cov[1 * 6 + 1]; - - Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); - - // eigen values and vectors are sorted in ascending order - ellipse_.long_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(1)); - ellipse_.short_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(0)); - - // principal component vector - const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); - ellipse_.yaw = std::atan2(pc_vector.y(), pc_vector.x()); - - // ellipse size along lateral direction (body-frame) - ellipse_.P = xy_covariance; - const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation); - ellipse_.size_lateral_direction = - scale_ * measure_size_ellipse_along_body_frame(ellipse_.P.inverse(), yaw_vehicle); - - const auto ellipse_marker = create_ellipse_marker(ellipse_, input_msg); + ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_); + + const auto ellipse_marker = autoware::localization_util::create_ellipse_marker( + ellipse_, input_msg->header, input_msg->pose); ellipse_marker_pub_->publish(ellipse_marker); // diagnostics @@ -134,16 +85,5 @@ void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr i diag_pub_->publish(diag_msg); } -double LocalizationErrorMonitor::measure_size_ellipse_along_body_frame( - const Eigen::Matrix2d & Pinv, const double theta) -{ - Eigen::MatrixXd e(2, 1); - e(0, 0) = std::cos(theta); - e(1, 0) = std::sin(theta); - - double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); - return d; -} - #include RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor) diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index 9a9086314f42e..ebf2b6dc10878 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -8,6 +8,7 @@ ament_auto_add_library(localization_util SHARED src/util_func.cpp src/smart_pose_buffer.cpp src/tree_structured_parzen_estimator.cpp + src/covariance_ellipse.cpp ) if(BUILD_TESTING) diff --git a/localization/localization_util/include/localization_util/covariance_ellipse.hpp b/localization/localization_util/include/localization_util/covariance_ellipse.hpp new file mode 100644 index 0000000000000..1f187d411dc63 --- /dev/null +++ b/localization/localization_util/include/localization_util/covariance_ellipse.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#define LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ + +#include + +#include +#include + +namespace autoware::localization_util +{ + +struct Ellipse +{ + double long_radius; + double short_radius; + double yaw; + Eigen::Matrix2d P; + double size_lateral_direction; +}; + +Ellipse calculate_xy_ellipse( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale); + +visualization_msgs::msg::Marker create_ellipse_marker( + const Ellipse & ellipse, const std_msgs::msg::Header & header, + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); + +} // namespace autoware::localization_util + +#endif // LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/localization_util/src/covariance_ellipse.cpp b/localization/localization_util/src/covariance_ellipse.cpp new file mode 100644 index 0000000000000..885ce2dcee19c --- /dev/null +++ b/localization/localization_util/src/covariance_ellipse.cpp @@ -0,0 +1,90 @@ +// Copyright 2024 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization_util/covariance_ellipse.hpp" + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace autoware::localization_util +{ + +Ellipse calculate_xy_ellipse( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) +{ + // input geometry_msgs::PoseWithCovariance contain 6x6 matrix + Eigen::Matrix2d xy_covariance; + const auto cov = pose_with_covariance.covariance; + xy_covariance(0, 0) = cov[0 * 6 + 0]; + xy_covariance(0, 1) = cov[0 * 6 + 1]; + xy_covariance(1, 0) = cov[1 * 6 + 0]; + xy_covariance(1, 1) = cov[1 * 6 + 1]; + + Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); + + Ellipse ellipse; + + // eigen values and vectors are sorted in ascending order + ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); + ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); + + // principal component vector + const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); + ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); + + // ellipse size along lateral direction (body-frame) + ellipse.P = xy_covariance; + const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); + const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); + Eigen::MatrixXd e(2, 1); + e(0, 0) = std::cos(yaw_vehicle); + e(1, 0) = std::sin(yaw_vehicle); + const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); + ellipse.size_lateral_direction = scale * d; + + return ellipse; +} + +visualization_msgs::msg::Marker create_ellipse_marker( + const Ellipse & ellipse, const std_msgs::msg::Header & header, + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) +{ + tf2::Quaternion quat; + quat.setEuler(0, 0, ellipse.yaw); + + const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); + const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); + visualization_msgs::msg::Marker marker; + marker.header = header; + marker.ns = "error_ellipse"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose_with_covariance.pose; + marker.pose.orientation = tf2::toMsg(quat); + marker.scale.x = ellipse_long_radius * 2; + marker.scale.y = ellipse_short_radius * 2; + marker.scale.z = 0.01; + marker.color.a = 0.1; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + return marker; +} + +} // namespace autoware::localization_util diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md index a28da699ad926..7b9397dfdba47 100644 --- a/localization/pose_estimator_arbiter/README.md +++ b/localization/pose_estimator_arbiter/README.md @@ -45,7 +45,7 @@ The following video demonstrates the switching of four different pose estimators Users can reproduce the demonstration using the following data and launch command: -[sample data (rosbag & map)](https://drive.google.com/file/d/1MxLo1Sw6PdvfkyOYf_9A5dZ9uli1vPvS/view) +[sample data (rosbag & map)](https://drive.google.com/file/d/1ZNlkyCtwe04iKFREdeZ5xuMU_jWpwM3W/view) The rosbag is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). The map is an edited version of the [original map data](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip) published on the AWSIM documentation page to make it suitable for multiple pose_estimators. diff --git a/localization/yabloc/yabloc_common/src/cv_decompress.cpp b/localization/yabloc/yabloc_common/src/cv_decompress.cpp index bf26908c9f0b0..3401f458f388a 100644 --- a/localization/yabloc/yabloc_common/src/cv_decompress.cpp +++ b/localization/yabloc/yabloc_common/src/cv_decompress.cpp @@ -17,7 +17,11 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif #include diff --git a/localization/yabloc/yabloc_common/src/pub_sub.cpp b/localization/yabloc/yabloc_common/src/pub_sub.cpp index 102a9012033fe..79d48b70ac172 100644 --- a/localization/yabloc/yabloc_common/src/pub_sub.cpp +++ b/localization/yabloc/yabloc_common/src/pub_sub.cpp @@ -14,7 +14,11 @@ #include "yabloc_common/pub_sub.hpp" -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif #include namespace yabloc::common diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 70e1bf2d6e6e2..c7d7aa67fd3ab 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -18,7 +18,11 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif #include namespace yabloc::line_segments_overlay diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 5590387e5ba26..46f5165c5f4e2 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -24,7 +24,11 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif #include diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index dd97ea2ad888f..39d5a81f6c109 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -20,7 +20,11 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif #include diff --git a/perception/bytetrack/lib/src/lapjv.cpp b/perception/bytetrack/lib/src/lapjv.cpp index ecf4c31b81ff8..1b8b39ccbb9f7 100644 --- a/perception/bytetrack/lib/src/lapjv.cpp +++ b/perception/bytetrack/lib/src/lapjv.cpp @@ -181,7 +181,8 @@ int_t _carr_dense( /** Find columns with minimum d[j] and put them on the SCAN list. */ -uint_t _find_dense(const uint_t n, uint_t lo, cost_t * d, int_t * cols, [[maybe_unused]] int_t * y) +uint_t _find_dense( + const uint_t n, uint_t lo, const cost_t * d, int_t * cols, [[maybe_unused]] int_t * y) { uint_t hi = lo + 1; cost_t mind = d[cols[lo]]; @@ -203,7 +204,7 @@ uint_t _find_dense(const uint_t n, uint_t lo, cost_t * d, int_t * cols, [[maybe_ // and try to decrease d of the TODO columns using the SCAN column. int_t _scan_dense( const uint_t n, cost_t * cost[], uint_t * plo, uint_t * phi, cost_t * d, int_t * cols, - int_t * pred, int_t * y, cost_t * v) + int_t * pred, const int_t * y, const cost_t * v) { uint_t lo = *plo; uint_t hi = *phi; diff --git a/perception/bytetrack/lib/src/strack.cpp b/perception/bytetrack/lib/src/strack.cpp index f2c5a40ad3c2e..81ba438faee44 100644 --- a/perception/bytetrack/lib/src/strack.cpp +++ b/perception/bytetrack/lib/src/strack.cpp @@ -112,12 +112,6 @@ void STrack::activate(int frame_id) this->track_id = this->next_id(); this->unique_id = boost::uuids::random_generator()(); - std::vector _tlwh_tmp(4); - _tlwh_tmp[0] = this->original_tlwh[0]; - _tlwh_tmp[1] = this->original_tlwh[1]; - _tlwh_tmp[2] = this->original_tlwh[2]; - _tlwh_tmp[3] = this->original_tlwh[3]; - std::vector xyah = tlwh_to_xyah(_tlwh_tmp); // init kf init_kalman_filter(); // reflect state @@ -132,7 +126,6 @@ void STrack::activate(int frame_id) void STrack::re_activate(STrack & new_track, int frame_id, bool new_id) { - std::vector xyah = tlwh_to_xyah(new_track.tlwh); // TODO(me): write kf update Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); measurement << new_track.tlwh[0], new_track.tlwh[1], new_track.tlwh[2], new_track.tlwh[3]; @@ -156,8 +149,6 @@ void STrack::update(STrack & new_track, int frame_id) this->frame_id = frame_id; this->tracklet_len++; - std::vector xyah = tlwh_to_xyah(new_track.tlwh); - // update // TODO(me): write update Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); diff --git a/perception/bytetrack/lib/src/utils.cpp b/perception/bytetrack/lib/src/utils.cpp index 64ad27cf36eba..e987bd36b8064 100644 --- a/perception/bytetrack/lib/src/utils.cpp +++ b/perception/bytetrack/lib/src/utils.cpp @@ -190,7 +190,6 @@ std::vector> ByteTracker::ious( // bbox_ious for (size_t k = 0; k < btlbrs.size(); k++) { - std::vector ious_tmp; float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1) * (btlbrs[k][3] - btlbrs[k][1] + 1); for (size_t n = 0; n < atlbrs.size(); n++) { float iw = std::min(atlbrs[n][2], btlbrs[k][2]) - std::max(atlbrs[n][0], btlbrs[k][0]) + 1; @@ -277,8 +276,6 @@ double ByteTracker::lapjv( std::vector> cost_c; cost_c.assign(cost.begin(), cost.end()); - std::vector> cost_c_extended; - int n_rows = cost.size(); int n_cols = cost[0].size(); rowsol.resize(n_rows); @@ -296,6 +293,8 @@ double ByteTracker::lapjv( } if (extend_cost || cost_limit < LONG_MAX) { + std::vector> cost_c_extended; + n = n_rows + n_cols; cost_c_extended.resize(n); for (size_t i = 0; i < cost_c_extended.size(); i++) cost_c_extended[i].resize(n); diff --git a/perception/bytetrack/src/bytetrack_visualizer_node.cpp b/perception/bytetrack/src/bytetrack_visualizer_node.cpp index ce350b8c288da..cdb767568a510 100644 --- a/perception/bytetrack/src/bytetrack_visualizer_node.cpp +++ b/perception/bytetrack/src/bytetrack_visualizer_node.cpp @@ -128,14 +128,14 @@ void ByteTrackVisualizerNode::callback( } std::vector bboxes; - for (auto & feat_obj : rect_msg->feature_objects) { + for (const auto & feat_obj : rect_msg->feature_objects) { auto roi_msg = feat_obj.feature.roi; cv::Rect rect(roi_msg.x_offset, roi_msg.y_offset, roi_msg.width, roi_msg.height); bboxes.push_back(rect); } std::vector uuids; - for (auto & dynamic_obj : uuid_msg->objects) { + for (const auto & dynamic_obj : uuid_msg->objects) { auto uuid_msg = dynamic_obj.id.uuid; boost::uuids::uuid uuid_raw; std::copy(uuid_msg.begin(), uuid_msg.end(), uuid_raw.begin()); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 65e03c6bc6316..d61ced593de78 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -18,6 +18,8 @@ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "image_projection_based_fusion/fusion_node.hpp" +#include + #include "autoware_perception_msgs/msg/object_classification.hpp" #include @@ -45,7 +47,8 @@ class RoiDetectedObjectFusionNode std::map generateDetectedObjectRoIs( const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection); + const Eigen::Affine3d & object2camera_affine, + const image_geometry::PinholeCameraModel & pinhole_camera_model); void fuseObjectsOnImage( const DetectedObjects & input_object_msg, diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 4cbc0990352e6..4d4fd8d2dac42 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -17,6 +17,8 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include + #include #include namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index c458c17bed793..26bcfd5ed802a 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -17,6 +17,8 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include + #include #include diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index ffa1666396a1d..107e0d6c179a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -42,6 +42,7 @@ #include "autoware_perception_msgs/msg/shape.hpp" #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" +#include #include #include #include @@ -60,6 +61,10 @@ struct PointData float distance; size_t orig_index; }; + +Eigen::Vector2d calcRawImageProjectedPoint( + const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d); + std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 8d5a2ef1fe519..00ffdc8fd5528 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -21,6 +21,7 @@ autoware_universe_utils cv_bridge euclidean_cluster + image_geometry image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 7973c22cc1d78..a7c83ac7614a8 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -307,9 +307,9 @@ void PointPaintingFusionNode::fuseOnSingleImage( const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; const auto p_step = painted_pointcloud_msg.point_step; // projection matrix - Eigen::Matrix3f camera_projection; // use only x,y,z - camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), - camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); + Eigen::Vector3f point_lidar, point_camera; /** dc : don't care @@ -342,15 +342,15 @@ dc | dc dc dc dc ||zc| continue; } // project - Eigen::Vector3f normalized_projected_point = camera_projection * Eigen::Vector3f(p_x, p_y, p_z); + Eigen::Vector2d projected_point = + calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z)); + // iterate 2d bbox for (const auto & feature_object : objects) { sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; // paint current point if it is inside bbox int label2d = feature_object.object.classification.front().label; - if ( - !isUnknown(label2d) && - isInsideBbox(normalized_projected_point.x(), normalized_projected_point.y(), roi, p_z)) { + if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, p_z)) { data = &painted_pointcloud_msg.data[0]; auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { @@ -361,7 +361,7 @@ dc | dc dc dc dc ||zc| #if 0 // Parallelizing loop don't support push_back if (debugger_) { - debug_image_points.push_back(normalized_projected_point); + debug_image_points.push_back(projected_point); } #endif } diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index bc786dd460d12..51987cbbcab84 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -83,10 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { - Eigen::Matrix4d projection; - projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), - camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), - camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -133,23 +131,19 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector4d projected_point = - projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + Eigen::Vector2d projected_point = + calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z)); if ( - 0 <= static_cast(normalized_projected_point.x()) && - static_cast(normalized_projected_point.x()) <= - static_cast(camera_info.width) - 1 && - 0 <= static_cast(normalized_projected_point.y()) && - static_cast(normalized_projected_point.y()) <= - static_cast(camera_info.height) - 1) { - min_x = std::min(static_cast(normalized_projected_point.x()), min_x); - min_y = std::min(static_cast(normalized_projected_point.y()), min_y); - max_x = std::max(static_cast(normalized_projected_point.x()), max_x); - max_y = std::max(static_cast(normalized_projected_point.y()), max_y); - projected_points.push_back(normalized_projected_point); - if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point); + 0 <= static_cast(projected_point.x()) && + static_cast(projected_point.x()) <= static_cast(camera_info.width) - 1 && + 0 <= static_cast(projected_point.y()) && + static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { + min_x = std::min(static_cast(projected_point.x()), min_x); + min_y = std::min(static_cast(projected_point.y()), min_y); + max_x = std::max(static_cast(projected_point.x()), max_x); + max_y = std::max(static_cast(projected_point.y()), max_y); + projected_points.push_back(projected_point); + if (debugger_) debugger_->obstacle_points_.push_back(projected_point); } } if (projected_points.empty()) { diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 762d3e0ee51b1..7d85ecb2f9200 100644 --- a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -86,15 +86,12 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( object2camera_affine = transformToEigen(transform_stamped_optional.value().transform); } - Eigen::Matrix4d camera_projection; - camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), - camera_info.p.at(3), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), - camera_info.p.at(7), camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), - camera_info.p.at(11); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); const auto object_roi_map = generateDetectedObjectRoIs( input_object_msg, static_cast(camera_info.width), - static_cast(camera_info.height), object2camera_affine, camera_projection); + static_cast(camera_info.height), object2camera_affine, pinhole_camera_model); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -109,7 +106,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection) + const Eigen::Affine3d & object2camera_affine, + const image_geometry::PinholeCameraModel & pinhole_camera_model) { std::map object_roi_map; int64_t timestamp_nsec = @@ -148,13 +146,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( continue; } - Eigen::Vector2d proj_point; - { - Eigen::Vector4d proj_point_hom = - camera_projection * Eigen::Vector4d(point.x(), point.y(), point.z(), 1.0); - proj_point = Eigen::Vector2d( - proj_point_hom.x() / (proj_point_hom.z()), proj_point_hom.y() / (proj_point_hom.z())); - } + Eigen::Vector2d proj_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z())); min_x = std::min(proj_point.x(), min_x); min_y = std::min(proj_point.y(), min_y); diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index ea5c2005bdd46..7f19402d9e565 100644 --- a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -101,10 +101,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } // transform pointcloud to camera optical frame id - Eigen::Matrix4d projection; - projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), - camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), - camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); + geometry_msgs::msg::TransformStamped transform_stamped; { const auto transform_stamped_optional = getTransformStamped( @@ -143,10 +142,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (transformed_z <= 0.0) { continue; } - Eigen::Vector4d projected_point = - projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); const auto & check_roi = feature_obj.feature.roi; @@ -156,10 +153,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( continue; } if ( - check_roi.x_offset <= normalized_projected_point.x() && - check_roi.y_offset <= normalized_projected_point.y() && - check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && - check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && + check_roi.x_offset + check_roi.width >= projected_point.x() && + check_roi.y_offset + check_roi.height >= projected_point.y()) { std::memcpy( &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step); clusters_data_size.at(i) += point_step; diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 0096b91f7a3b6..06cc0fd2c61f2 100644 --- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -65,11 +65,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( if (mask.cols == 0 || mask.rows == 0) { return; } - Eigen::Matrix4d projection; - projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), - camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), - camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0, - 0.0, 1.0; + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(camera_info); + geometry_msgs::msg::TransformStamped transform_stamped; // transform pointcloud from frame id to camera optical frame id { @@ -113,14 +111,11 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector4d projected_point = - projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + Eigen::Vector2d projected_point = calcRawImageProjectedPoint( + pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z)); - bool is_inside_image = - normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && - normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; + bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && + projected_point.y() > 0 && projected_point.y() < camera_info.height; if (!is_inside_image) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); @@ -129,8 +124,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( // skip filtering pointcloud where semantic id out of the defined list uint8_t semantic_id = mask.at( - static_cast(normalized_projected_point.y()), - static_cast(normalized_projected_point.x())); + static_cast(projected_point.y()), static_cast(projected_point.x())); if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 2e997f3e8e60f..aeec2886a801a 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,8 +13,18 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" + namespace image_projection_based_fusion { +Eigen::Vector2d calcRawImageProjectedPoint( + const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d) +{ + const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d); + + const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point); + + return Eigen::Vector2d(raw_image_point.x, raw_image_point.y); +} std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp index fde4fcbee3d6c..e75aff416b8aa 100644 --- a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp @@ -94,12 +94,12 @@ TEST_F(PostprocessKernelTest, SingleDetectionTest) constexpr float detection_x = 70.f; constexpr float detection_y = -38.4f; constexpr float detection_z = 1.0; - constexpr float detection_log_w = std::log(7.0); - constexpr float detection_log_l = std::log(1.0); - constexpr float detection_log_h = std::log(2.0); + const float detection_log_w = std::log(7.0); + const float detection_log_l = std::log(1.0); + const float detection_log_h = std::log(2.0); constexpr float detection_yaw = M_PI_4; - constexpr float detection_yaw_sin = std::sin(detection_yaw); - constexpr float detection_yaw_cos = std::sin(detection_yaw); + const float detection_yaw_sin = std::sin(detection_yaw); + const float detection_yaw_cos = std::sin(detection_yaw); constexpr float detection_vel_x = 5.0; constexpr float detection_vel_y = -5.0; @@ -240,9 +240,9 @@ TEST_F(PostprocessKernelTest, CircleNMSTest) constexpr float detection_x = 70.f; constexpr float detection_y = -38.4f; constexpr float detection_z = 1.0; - constexpr float detection_log_w = std::log(7.0); - constexpr float detection_log_l = std::log(1.0); - constexpr float detection_log_h = std::log(2.0); + const float detection_log_w = std::log(7.0); + const float detection_log_l = std::log(1.0); + const float detection_log_h = std::log(2.0); constexpr float detection_yaw1_sin = 0.0; constexpr float detection_yaw1_cos = 1.0; constexpr float detection_yaw2_sin = 1.0; diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/object_velocity_splitter/CMakeLists.txt index 68e87a3355f86..afad278e383d8 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/object_velocity_splitter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(object_velocity_splitter_node_component SHARED - src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/object_velocity_splitter_node.cpp ) -rclcpp_components_register_node(object_velocity_splitter_node_component - PLUGIN "object_velocity_splitter::ObjectVelocitySplitterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::object_velocity_splitter::ObjectVelocitySplitterNode" EXECUTABLE object_velocity_splitter_node ) diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp similarity index 93% rename from perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp rename to perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp index e15871d18840f..a10692faeff1e 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "object_velocity_splitter/object_velocity_splitter_node.hpp" +#include "object_velocity_splitter_node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" @@ -40,7 +40,7 @@ bool update_param( } } // namespace -namespace object_velocity_splitter +namespace autoware::object_velocity_splitter { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -110,7 +110,7 @@ rcl_interfaces::msg::SetParametersResult ObjectVelocitySplitterNode::onSetParam( return result; } -} // namespace object_velocity_splitter +} // namespace autoware::object_velocity_splitter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(object_velocity_splitter::ObjectVelocitySplitterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::object_velocity_splitter::ObjectVelocitySplitterNode) diff --git a/perception/object_velocity_splitter/include/object_velocity_splitter/object_velocity_splitter_node.hpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp similarity index 85% rename from perception/object_velocity_splitter/include/object_velocity_splitter/object_velocity_splitter_node.hpp rename to perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp index 2180b467dbbcf..ca0a9a9e65f3c 100644 --- a/perception/object_velocity_splitter/include/object_velocity_splitter/object_velocity_splitter_node.hpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OBJECT_VELOCITY_SPLITTER__OBJECT_VELOCITY_SPLITTER_NODE_HPP_ -#define OBJECT_VELOCITY_SPLITTER__OBJECT_VELOCITY_SPLITTER_NODE_HPP_ +#ifndef OBJECT_VELOCITY_SPLITTER_NODE_HPP_ +#define OBJECT_VELOCITY_SPLITTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -24,7 +24,7 @@ #include #include -namespace object_velocity_splitter +namespace autoware::object_velocity_splitter { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -62,6 +62,6 @@ class ObjectVelocitySplitterNode : public rclcpp::Node NodeParam node_param_{}; }; -} // namespace object_velocity_splitter +} // namespace autoware::object_velocity_splitter -#endif // OBJECT_VELOCITY_SPLITTER__OBJECT_VELOCITY_SPLITTER_NODE_HPP_ +#endif // OBJECT_VELOCITY_SPLITTER_NODE_HPP_ diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index bf4a46cda7ae6..6414739413d5c 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(radar_crossing_objects_noise_filter_node_component SHARED - src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME}_node_component SHARED + src/radar_crossing_objects_noise_filter_node.cpp ) -rclcpp_components_register_node(radar_crossing_objects_noise_filter_node_component - PLUGIN "radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode" +rclcpp_components_register_node(${PROJECT_NAME}_node_component + PLUGIN "autoware::radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode" EXECUTABLE radar_crossing_objects_noise_filter_node ) @@ -23,10 +23,10 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(radar_crossing_objects_noise_filter ${test_files}) + ament_add_ros_isolated_gtest(${PROJECT_NAME} ${test_files}) - target_link_libraries(radar_crossing_objects_noise_filter - radar_crossing_objects_noise_filter_node_component + target_link_libraries(${PROJECT_NAME} + ${PROJECT_NAME}_node_component ) endif() diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/radar_crossing_objects_noise_filter/package.xml index 0db40b24e809c..d0245dfe62a40 100644 --- a/perception/radar_crossing_objects_noise_filter/package.xml +++ b/perception/radar_crossing_objects_noise_filter/package.xml @@ -16,7 +16,6 @@ autoware_perception_msgs autoware_universe_utils - geometry_msgs rclcpp rclcpp_components tf2 diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp similarity index 94% rename from perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp rename to perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp index 2f5884c04ce37..13231b83bf27b 100644 --- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" +#include "radar_crossing_objects_noise_filter_node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" @@ -50,7 +50,7 @@ bool update_param( } } // namespace -namespace radar_crossing_objects_noise_filter +namespace autoware::radar_crossing_objects_noise_filter { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -142,8 +142,8 @@ bool RadarCrossingObjectsNoiseFilterNode::isNoise(const DetectedObject & object) } } -} // namespace radar_crossing_objects_noise_filter +} // namespace autoware::radar_crossing_objects_noise_filter #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE( - radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode) + autoware::radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode) diff --git a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp similarity index 82% rename from perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp rename to perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp index fbd64c884d396..6e389f563a138 100644 --- a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_CROSSING_OBJECTS_NOISE_FILTER__RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_ -#define RADAR_CROSSING_OBJECTS_NOISE_FILTER__RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_ +#ifndef RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_ +#define RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -23,7 +23,7 @@ #include #include -namespace radar_crossing_objects_noise_filter +namespace autoware::radar_crossing_objects_noise_filter { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -63,6 +63,6 @@ class RadarCrossingObjectsNoiseFilterNode : public rclcpp::Node bool isNoise(const DetectedObject & object); }; -} // namespace radar_crossing_objects_noise_filter +} // namespace autoware::radar_crossing_objects_noise_filter -#endif // RADAR_CROSSING_OBJECTS_NOISE_FILTER__RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_ +#endif // RADAR_CROSSING_OBJECTS_NOISE_FILTER_NODE_HPP_ diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp index 44ccaae8ee7fc..93475af7b3628 100644 --- a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -12,24 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "../../src/radar_crossing_objects_noise_filter_node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" -#include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" #include #include -std::shared_ptr get_node( - double angle_threshold, double velocity_threshold) +std::shared_ptr +get_node(double angle_threshold, double velocity_threshold) { rclcpp::NodeOptions node_options; node_options.parameter_overrides( {{"angle_threshold", angle_threshold}, {"velocity_threshold", velocity_threshold}}); - auto node = - std::make_shared( - node_options); + auto node = std::make_shared< + autoware::radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode>( + node_options); return node; } diff --git a/perception/radar_fusion_to_detected_object/CMakeLists.txt b/perception/radar_fusion_to_detected_object/CMakeLists.txt index c8297567511dd..76c27d3958841 100644 --- a/perception/radar_fusion_to_detected_object/CMakeLists.txt +++ b/perception/radar_fusion_to_detected_object/CMakeLists.txt @@ -6,14 +6,14 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(radar_object_fusion_to_detected_object_node_component SHARED - src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/node.cpp src/radar_fusion_to_detected_object.cpp ) -rclcpp_components_register_node(radar_object_fusion_to_detected_object_node_component - PLUGIN "radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode" - EXECUTABLE radar_object_fusion_to_detected_object_node +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode" + EXECUTABLE radar_fusion_to_detected_object_node ) # Tests diff --git a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml b/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml index d43f6b9655ae5..e5505ad87d9ec 100644 --- a/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml +++ b/perception/radar_fusion_to_detected_object/launch/radar_object_fusion_to_detected_object.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index aa0711f1cc6ce..2094ff7770555 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -22,7 +22,6 @@ message_filters rclcpp rclcpp_components - std_msgs ament_lint_common autoware_lint_common diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp b/perception/radar_fusion_to_detected_object/src/include/node.hpp similarity index 86% rename from perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp rename to perception/radar_fusion_to_detected_object/src/include/node.hpp index 96794509bdfc3..3f181da0041cb 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp +++ b/perception/radar_fusion_to_detected_object/src/include/node.hpp @@ -13,13 +13,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ -#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#ifndef NODE_HPP_ +#define NODE_HPP_ #include "message_filters/subscriber.h" #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" -#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" +#include "radar_fusion_to_detected_object.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_perception_msgs/msg/detected_objects.hpp" @@ -29,7 +29,7 @@ #include #include -namespace radar_fusion_to_detected_object +namespace autoware::radar_fusion_to_detected_object { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -87,6 +87,6 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node const DetectedObject & radar_object, const std_msgs::msg::Header & header_); }; -} // namespace radar_fusion_to_detected_object +} // namespace autoware::radar_fusion_to_detected_object -#endif // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_NODE_HPP_ +#endif // NODE_HPP_ diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp similarity index 91% rename from perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp rename to perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp index cd7e7de6b2ff4..545b7d7e41b2a 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp @@ -12,25 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ -#define RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#ifndef RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ + +#define EIGEN_MPL2_ONLY #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "rclcpp/logger.hpp" -#define EIGEN_MPL2_ONLY + #include #include #include "autoware_perception_msgs/msg/detected_objects.hpp" #include "geometry_msgs/msg/pose_with_covariance.hpp" #include "geometry_msgs/msg/twist_with_covariance.hpp" -// #include "std_msgs/msg/header.hpp" #include #include #include -namespace radar_fusion_to_detected_object +namespace autoware::radar_fusion_to_detected_object { using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::Point2d; @@ -113,6 +114,6 @@ class RadarFusionToDetectedObject double getTwistNorm(const Twist & twist); LinearRing2d createObject2dWithMargin(const Point2d object_size, const double margin); }; -} // namespace radar_fusion_to_detected_object +} // namespace autoware::radar_fusion_to_detected_object -#endif // RADAR_FUSION_TO_DETECTED_OBJECT__RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ +#endif // RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/node.cpp similarity index 96% rename from perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp rename to perception/radar_fusion_to_detected_object/src/node.cpp index 90851674ef9eb..58e893032340c 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/node.cpp @@ -13,7 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object_node.hpp" +#include "include/node.hpp" + #include "rclcpp/rclcpp.hpp" #include @@ -46,7 +47,7 @@ bool update_param( } } // namespace -namespace radar_fusion_to_detected_object +namespace autoware::radar_fusion_to_detected_object { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -217,8 +218,8 @@ RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::s return output; } -} // namespace radar_fusion_to_detected_object +} // namespace autoware::radar_fusion_to_detected_object #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE( - radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode) + autoware::radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode) diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 5a0c1b5412312..0c90a468f2ebd 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_fusion_to_detected_object/radar_fusion_to_detected_object.hpp" +#include "include/radar_fusion_to_detected_object.hpp" -#include -#include +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/math/normalization.hpp" #include @@ -27,7 +27,7 @@ #include #include -namespace radar_fusion_to_detected_object +namespace autoware::radar_fusion_to_detected_object { using autoware::universe_utils::LinearRing2d; using autoware::universe_utils::Point2d; @@ -358,4 +358,4 @@ LinearRing2d RadarFusionToDetectedObject::createObject2dWithMargin( return box; } -} // namespace radar_fusion_to_detected_object +} // namespace autoware::radar_fusion_to_detected_object diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/radar_object_clustering/CMakeLists.txt index ea25353d3e1d7..9a54bbb0dae43 100644 --- a/perception/radar_object_clustering/CMakeLists.txt +++ b/perception/radar_object_clustering/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(radar_object_clustering_node_component SHARED - src/radar_object_clustering_node/radar_object_clustering_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_object_clustering_node.cpp ) -rclcpp_components_register_node(radar_object_clustering_node_component - PLUGIN "radar_object_clustering::RadarObjectClusteringNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_object_clustering::RadarObjectClusteringNode" EXECUTABLE radar_object_clustering_node ) diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node.cpp similarity index 97% rename from perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp rename to perception/radar_object_clustering/src/radar_object_clustering_node.cpp index 339ab21741d29..b867d6a193ed7 100644 --- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_object_clustering/radar_object_clustering_node.hpp" +#include "radar_object_clustering_node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" @@ -20,17 +20,18 @@ #include -#include -#include -#include -#include - #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif +#include +#include +#include +#include +#include + namespace { template @@ -58,7 +59,7 @@ double get_distance(const autoware_perception_msgs::msg::DetectedObject & object } // namespace -namespace radar_object_clustering +namespace autoware::radar_object_clustering { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -229,7 +230,7 @@ rcl_interfaces::msg::SetParametersResult RadarObjectClusteringNode::onSetParam( result.reason = "success"; return result; } -} // namespace radar_object_clustering +} // namespace autoware::radar_object_clustering #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_object_clustering::RadarObjectClusteringNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_object_clustering::RadarObjectClusteringNode) diff --git a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp b/perception/radar_object_clustering/src/radar_object_clustering_node.hpp similarity index 87% rename from perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp rename to perception/radar_object_clustering/src/radar_object_clustering_node.hpp index 38a2235a117eb..13284a4151f49 100644 --- a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ -#define RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ +#ifndef RADAR_OBJECT_CLUSTERING_NODE_HPP_ +#define RADAR_OBJECT_CLUSTERING_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -24,7 +24,7 @@ #include #include -namespace radar_object_clustering +namespace autoware::radar_object_clustering { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -72,6 +72,6 @@ class RadarObjectClusteringNode : public rclcpp::Node bool isSameObject(const DetectedObject & object_1, const DetectedObject & object_2); }; -} // namespace radar_object_clustering +} // namespace autoware::radar_object_clustering -#endif // RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ +#endif // RADAR_OBJECT_CLUSTERING_NODE_HPP_ diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index 1bc548fa7a951..c6bd93b7a9371 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -137,22 +137,8 @@ LinearMotionTracker::LinearMotionTracker( } else { P_v_xy = R * P_v_xy_local * R.transpose(); } - // acceleration covariance often written in object frame - const bool has_acceleration_covariance = - false; // currently message does not have acceleration covariance - if (has_acceleration_covariance) { - // const auto ax_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; // This - // is future update - // const auto ay_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This - // is future update - // Eigen::Matrix2d P_a_xy_local; - // P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov; - P_a_xy = R * P_a_xy_local * R.transpose(); - } else { - P_a_xy = R * P_a_xy_local * R.transpose(); - } + + P_a_xy = R * P_a_xy_local * R.transpose(); // put value in P matrix // use block description. This assume x,y,vx,vy,ax,ay in this order diff --git a/perception/radar_tracks_msgs_converter/CMakeLists.txt b/perception/radar_tracks_msgs_converter/CMakeLists.txt index 766fb4b939904..4ff3be1fa45a4 100644 --- a/perception/radar_tracks_msgs_converter/CMakeLists.txt +++ b/perception/radar_tracks_msgs_converter/CMakeLists.txt @@ -5,12 +5,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() ## Targets -ament_auto_add_library(radar_tracks_msgs_converter_node_component SHARED - src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/radar_tracks_msgs_converter_node.cpp ) -rclcpp_components_register_node(radar_tracks_msgs_converter_node_component - PLUGIN "radar_tracks_msgs_converter::RadarTracksMsgsConverterNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::radar_tracks_msgs_converter::RadarTracksMsgsConverterNode" EXECUTABLE radar_tracks_msgs_converter_node ) diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml index ad0257dbbd726..534c9a20fa0df 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/radar_tracks_msgs_converter/package.xml @@ -17,14 +17,12 @@ autoware_perception_msgs autoware_universe_utils - geometry_msgs nav_msgs radar_msgs rclcpp rclcpp_components tf2 tf2_geometry_msgs - tf2_ros ament_lint_auto autoware_lint_common diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp similarity index 98% rename from perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp rename to perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp index 36f4d3cd3da90..5d45595733c4a 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp" +#include "radar_tracks_msgs_converter_node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" @@ -56,7 +56,7 @@ bool update_param( } } // namespace -namespace radar_tracks_msgs_converter +namespace autoware::radar_tracks_msgs_converter { enum class RadarTrackObjectID { @@ -407,7 +407,7 @@ uint8_t RadarTracksMsgsConverterNode::convertClassification(const uint16_t class } } -} // namespace radar_tracks_msgs_converter +} // namespace autoware::radar_tracks_msgs_converter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(radar_tracks_msgs_converter::RadarTracksMsgsConverterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::radar_tracks_msgs_converter::RadarTracksMsgsConverterNode) diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp similarity index 93% rename from perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp rename to perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp index 30d02c90b0594..36528711cc403 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ -#define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ +#ifndef RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ +#define RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ #include "autoware/universe_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" @@ -32,7 +32,7 @@ #include #include -namespace radar_tracks_msgs_converter +namespace autoware::radar_tracks_msgs_converter { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjectKinematics; @@ -114,6 +114,6 @@ class RadarTracksMsgsConverterNode : public rclcpp::Node uint8_t convertClassification(const uint16_t classification); }; -} // namespace radar_tracks_msgs_converter +} // namespace autoware::radar_tracks_msgs_converter -#endif // RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ +#endif // RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/simple_object_merger/CMakeLists.txt index 10bd0efa7b8fc..1a65bdf96050e 100644 --- a/perception/simple_object_merger/CMakeLists.txt +++ b/perception/simple_object_merger/CMakeLists.txt @@ -6,12 +6,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Targets -ament_auto_add_library(simple_object_merger_node_component SHARED - src/simple_object_merger_node/simple_object_merger_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/simple_object_merger_node.cpp ) -rclcpp_components_register_node(simple_object_merger_node_component - PLUGIN "simple_object_merger::SimpleObjectMergerNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::simple_object_merger::SimpleObjectMergerNode" EXECUTABLE simple_object_merger_node ) diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node.cpp similarity index 96% rename from perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp rename to perception/simple_object_merger/src/simple_object_merger_node.cpp index faa531560ba09..98c7fd03c1e99 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_object_merger/simple_object_merger_node.hpp" +#include "simple_object_merger_node.hpp" #include @@ -65,7 +65,7 @@ autoware_perception_msgs::msg::DetectedObjects::SharedPtr getTransformedObjects( } // namespace -namespace simple_object_merger +namespace autoware::simple_object_merger { using namespace std::literals; using std::chrono::duration; @@ -193,7 +193,7 @@ void SimpleObjectMergerNode::onTimer() pub_objects_->publish(output_objects); } -} // namespace simple_object_merger +} // namespace autoware::simple_object_merger #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(simple_object_merger::SimpleObjectMergerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::simple_object_merger::SimpleObjectMergerNode) diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/src/simple_object_merger_node.hpp similarity index 89% rename from perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp rename to perception/simple_object_merger/src/simple_object_merger_node.hpp index d25eef20cc676..d5f907ff09ef2 100644 --- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp +++ b/perception/simple_object_merger/src/simple_object_merger_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ -#define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ +#ifndef SIMPLE_OBJECT_MERGER_NODE_HPP_ +#define SIMPLE_OBJECT_MERGER_NODE_HPP_ #include "autoware/universe_utils/ros/transform_listener.hpp" #include "rclcpp/rclcpp.hpp" @@ -25,7 +25,7 @@ #include #include -namespace simple_object_merger +namespace autoware::simple_object_merger { using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; @@ -76,6 +76,6 @@ class SimpleObjectMergerNode : public rclcpp::Node size_t input_topic_size; }; -} // namespace simple_object_merger +} // namespace autoware::simple_object_merger -#endif // SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ +#endif // SIMPLE_OBJECT_MERGER_NODE_HPP_ diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md index ca407b1ff6811..af88e73cc04a6 100644 --- a/perception/tensorrt_yolox/README.md +++ b/perception/tensorrt_yolox/README.md @@ -2,13 +2,13 @@ ## Purpose -This package detects target objects e.g., cars, trucks, bicycles, and pedestrians on a image based on [YOLOX](https://github.com/Megvii-BaseDetection/YOLOX) model. +This package detects target objects e.g., cars, trucks, bicycles, and pedestrians and segment target objects such as cars, trucks, buses and pedestrian, building, vegetation, road, sidewalk on a image based on [YOLOX](https://github.com/Megvii-BaseDetection/YOLOX) model with multi-header structure. ## Inner-workings / Algorithms ### Cite - + Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Series in 2021", arXiv preprint arXiv:2107.08430, 2021 [[ref](https://arxiv.org/abs/2107.08430)] @@ -22,10 +22,12 @@ Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Se ### Output -| Name | Type | Description | -| ------------- | -------------------------------------------------- | -------------------------------------------------- | -| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | -| `out/image` | `sensor_msgs/Image` | The image with 2D bounding boxes for visualization | +| Name | Type | Description | +| ---------------- | -------------------------------------------------- | ------------------------------------------------------------------- | +| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | +| `out/image` | `sensor_msgs/Image` | The image with 2D bounding boxes for visualization | +| `out/mask` | `sensor_msgs/Image` | The semantic segmentation mask | +| `out/color_mask` | `sensor_msgs/Image` | The colorized image of semantic segmentation mask for visualization | ## Parameters @@ -40,20 +42,33 @@ Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Se ### Node Parameters -| Name | Type | Default Value | Description | -| ----------------------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `model_path` | string | "" | The onnx file name for yolox model | -| `label_path` | string | "" | The label file with label names for detected objects written on it | -| `precision` | string | "fp16" | The inference mode: "fp32", "fp16", "int8" | -| `build_only` | bool | false | shutdown node after TensorRT engine file is built | -| `calibration_algorithm` | string | "MinMax" | Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: Entropy",("Legacy" \| "Percentile"), "MinMax"] | -| `dla_core_id` | int | -1 | If positive ID value is specified, the node assign inference task to the DLA core | -| `quantize_first_layer` | bool | false | If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8 | -| `quantize_last_layer` | bool | false | If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8 | -| `profile_per_layer` | bool | false | If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose. | -| `clip_value` | double | 0.0 | If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration | -| `preprocess_on_gpu` | bool | true | If true, pre-processing is performed on GPU | -| `calibration_image_list_path` | string | "" | Path to a file which contains path to images. Those images will be used for int8 quantization. | +| Name | Type | Default Value | Description | +| -------------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `model_path` | string | "" | The onnx file name for yolox model | +| `model_name` | string | "" | The yolox model name:
"yolox-sPlus-T4-960x960-pseudo-finetune" for detection only, could reduce resource and processing_time
"yolox-sPlus-opt-pseudoV2-T4-960x960-T4-seg16cls" for multi-task including semantic segmentation | +| `label_path` | string | "" | The label file with label names for detected objects written on it | +| `precision` | string | "fp16" | The inference mode: "fp32", "fp16", "int8" | +| `build_only` | bool | false | shutdown node after TensorRT engine file is built | +| `calibration_algorithm` | string | "MinMax" | Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: Entropy",("Legacy" \| "Percentile"), "MinMax"] | +| `dla_core_id` | int | -1 | If positive ID value is specified, the node assign inference task to the DLA core | +| `quantize_first_layer` | bool | false | If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8 | +| `quantize_last_layer` | bool | false | If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8 | +| `profile_per_layer` | bool | false | If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose. | +| `clip_value` | double | 0.0 | If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration | +| `preprocess_on_gpu` | bool | true | If true, pre-processing is performed on GPU | +| `calibration_image_list_path` | string | "" | Path to a file which contains path to images. Those images will be used for int8 quantization. | +| `yolox_s_plus_opt_param_path` | string | "" | Path to parameter file | +| `is_publish_color_mask` | bool | false | If true, publish color mask for result visualization | +| `is_roi_overlap_segment` | bool | true | If true, overlay detected object roi onto semantic segmentation to avoid over-filtering pointcloud especially small size objects | +| `overlap_roi_score_threshold` | float | 0.3 | minimum existence_probability of detected roi considered to replace segmentation | +| `roi_overlay_segment_label.UNKNOWN` | bool | true | If true, unknown objects roi will be overlaid onto sematic segmentation mask. | +| `roi_overlay_segment_label.CAR` | bool | false | If true, car objects roi will be overlaid onto sematic segmentation mask. | +| `roi_overlay_segment_label.TRUCK` | bool | false | If true, truck objects roi will be overlaid onto sematic segmentation mask. | +| `roi_overlay_segment_label.BUS` | bool | false | If true, bus objects roi will be overlaid onto sematic segmentation mask. | +| `roi_overlay_segment_label.MOTORCYCLE` | bool | true | If true, motorcycle objects roi will be overlaid onto sematic segmentation mask. | +| `roi_overlay_segment_label.BICYCLE` | bool | true | If true, bicycle objects roi will be overlaid onto sematic segmentation mask. | +| `roi_overlay_segment_label.PEDESTRIAN` | bool | true | If true, pedestrian objects roi will be overlaid onto sematic segmentation mask. | +| `roi_overlay_segment_label.ANIMAL` | bool | true | If true, animal objects roi will be overlaid onto sematic segmentation mask. | ## Assumptions / Known limits @@ -69,6 +84,27 @@ The label contained in detected 2D bounding boxes (i.e., `out/objects`) will be If other labels (case insensitive) are contained in the file specified via the `label_file` parameter, those are labeled as `UNKNOWN`, while detected rectangles are drawn in the visualization result (`out/image`). +The semantic segmentation mask is a gray image whose each pixel is index of one following class: + +| index | semantic name | +| ----- | ---------------- | +| 0 | road | +| 1 | building | +| 2 | wall | +| 3 | obstacle | +| 4 | traffic_light | +| 5 | traffic_sign | +| 6 | person | +| 7 | vehicle | +| 8 | bike | +| 9 | road | +| 10 | sidewalk | +| 11 | roadPaint | +| 12 | curbstone | +| 13 | crosswalk_others | +| 14 | vegetation | +| 15 | sky | + ## Onnx model A sample model (named `yolox-tiny.onnx`) is downloaded by ansible script on env preparation stage, if not, please, follow [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). @@ -79,11 +115,12 @@ hence these parameters are ignored when users specify ONNX models including this This package accepts both `EfficientNMS_TRT` attached ONNXs and [models published from the official YOLOX repository](https://github.com/Megvii-BaseDetection/YOLOX/tree/main/demo/ONNXRuntime#download-onnx-models) (we referred to them as "plain" models). -In addition to `yolox-tiny.onnx`, a custom model named `yolox-sPlus-opt.onnx` is either available. -This model is based on YOLOX-s and tuned to perform more accurate detection with almost comparable execution speed with `yolox-tiny`. +In addition to `yolox-tiny.onnx`, a custom model named `yolox-sPlus-opt-pseudoV2-T4-960x960-T4-seg16cls` is either available. +This model is multi-header structure model which is based on YOLOX-s and tuned to perform more accurate detection with almost comparable execution speed with `yolox-tiny`. To get better results with this model, users are recommended to use some specific running arguments such as `precision:=int8`, `calibration_algorithm:=Entropy`, `clip_value:=6.0`. Users can refer `launch/yolox_sPlus_opt.launch.xml` to see how this model can be used. +Beside detection result, this model also output image semantic segmentation result for pointcloud filtering purpose. All models are automatically converted to TensorRT format. These converted files will be saved in the same directory as specified ONNX files @@ -146,7 +183,7 @@ Please refer [the official document](https://github.com/Megvii-BaseDetection/YOL ## Label file -A sample label file (named `label.txt`)is also downloaded automatically during env preparation process +A sample label file (named `label.txt`) and semantic segmentation color map file (name `semseg_color_map.csv`) are also downloaded automatically during env preparation process (**NOTE:** This file is incompatible with models that output labels for the COCO dataset (e.g., models from the official YOLOX repository)). This file represents the correspondence between class index (integer outputted from YOLOX network) and @@ -157,3 +194,4 @@ with labels according to the order in this file. - - +- diff --git a/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml b/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml index bc67173442094..57c1b40c44a4d 100644 --- a/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml +++ b/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml @@ -1,7 +1,29 @@ +# cspell: ignore semseg /**: ros__parameters: + # refine segmentation mask by overlay roi class + # disable when sematic segmentation accuracy is good enough + is_roi_overlap_segment: true + + # minimum existence_probability of detected roi considered to replace segmentation + overlap_roi_score_threshold: 0.3 + + # publish color mask for result visualization + is_publish_color_mask: false + + roi_overlay_segment_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + MOTORCYCLE : true + BICYCLE : true + PEDESTRIAN : true + ANIMAL: true + model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx" label_path: "$(var data_path)/tensorrt_yolox/label.txt" + color_map_path: "$(var data_path)/tensorrt_yolox/semseg_color_map.csv" score_threshold: 0.35 nms_threshold: 0.7 precision: "int8" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]. diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp index 3549ae35e70ea..faac6de4e3284 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/preprocess.hpp @@ -179,6 +179,21 @@ extern void crop_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( extern void multi_scale_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( float * dst, unsigned char * src, int d_w, int d_h, int d_c, Roi * d_roi, int s_w, int s_h, int s_c, int batch, float norm, cudaStream_t stream); -} // namespace tensorrt_yolox +/** + * @brief Argmax on GPU + * @param[out] dst processed image + * @param[in] src probability map + * @param[in] d_w width for output + * @param[in] d_h height for output + * @param[in] s_w width for input + * @param[in] s_h height for input + * @param[in] s_c channel for input + * @param[in] batch batch size + * @param[in] stream cuda stream + */ +extern void argmax_gpu( + unsigned char * dst, float * src, int d_w, int d_h, int s_w, int s_h, int s_c, int batch, + cudaStream_t stream); +} // namespace tensorrt_yolox #endif // TENSORRT_YOLOX__PREPROCESS_HPP_ diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp index c42222a70c96b..d287c8a44d4cf 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp @@ -52,6 +52,13 @@ struct GridAndStride int stride; }; +typedef struct Colormap_ +{ + int id; + std::string name; + std::vector color; +} Colormap; + /** * @class TrtYoloX * @brief TensorRT YOLOX for faster inference @@ -85,7 +92,7 @@ class TrtYoloX const bool use_gpu_preprocess = false, std::string calibration_image_list_file = std::string(), const double norm_factor = 1.0, [[maybe_unused]] const std::string & cache_dir = "", const tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, - const size_t max_workspace_size = (1 << 30)); + const size_t max_workspace_size = (1 << 30), const std::string & color_map_path = ""); /** * @brief Deconstruct TrtYoloX */ @@ -96,7 +103,9 @@ class TrtYoloX * @param[out] objects results for object detection * @param[in] images batched images */ - bool doInference(const std::vector & images, ObjectArrays & objects); + bool doInference( + const std::vector & images, ObjectArrays & objects, std::vector & masks, + std::vector & color_masks); /** * @brief run inference including pre-process and post-process @@ -130,6 +139,22 @@ class TrtYoloX */ void printProfiling(void); + /** + * @brief get num for multitask heads + */ + int getMultitaskNum(void); + + /** + * @brief get colorized masks from index using specific colormap + * @param[out] cmask colorized mask + * @param[in] index multitask index + * @param[in] colormap colormap for masks + */ + void getColorizedMask( + const std::vector & colormap, const cv::Mat & mask, + cv::Mat & colorized_mask); + inline std::vector getColorMap() { return sematic_color_map_; } + private: /** * @brief run preprocess including resizing, letterbox, NHWC2NCHW and toFloat on CPU @@ -177,7 +202,9 @@ class TrtYoloX const cv::Mat & images, int batch_size, ObjectArrays & objects); bool feedforward(const std::vector & images, ObjectArrays & objects); - bool feedforwardAndDecode(const std::vector & images, ObjectArrays & objects); + bool feedforwardAndDecode( + const std::vector & images, ObjectArrays & objects, std::vector & masks, + std::vector & color_masks); void decodeOutputs(float * prob, ObjectArray & objects, float scale, cv::Size & img_size) const; void generateGridsAndStride( const int target_w, const int target_h, const std::vector & strides, @@ -206,6 +233,26 @@ class TrtYoloX void nmsSortedBboxes( const ObjectArray & face_objects, std::vector & picked, float nms_threshold) const; + /** + * @brief get a mask image for a segmentation head + * @param[out] argmax argmax results + * @param[in] prob probability map + * @param[in] dims dimension for probability map + * @param[in] out_w mask width excluding letterbox + * @param[in] out_h mask height excluding letterbox + */ + cv::Mat getMaskImage(float * prob, nvinfer1::Dims dims, int out_w, int out_h); + + /** + * @brief get a mask image on GPUs for a segmentation head + * @param[out] mask image + * @param[in] prob probability map on device + * @param[in] out_w mask width excluding letterbox + * @param[in] out_h mask height excluding letterbox + * @param[in] b current batch + */ + cv::Mat getMaskImageGpu(float * d_prob, nvinfer1::Dims dims, int out_w, int out_h, int b); + std::unique_ptr trt_common_; std::vector input_h_; @@ -249,6 +296,20 @@ class TrtYoloX CudaUniquePtrHost roi_h_; // device pointer for ROI CudaUniquePtr roi_d_; + + // flag whether model has multitasks + int multitask_; + // buff size for segmentation heads + CudaUniquePtr segmentation_out_prob_d_; + CudaUniquePtrHost segmentation_out_prob_h_; + size_t segmentation_out_elem_num_; + size_t segmentation_out_elem_num_per_batch_; + std::vector segmentation_masks_; + // host buffer for argmax postprocessing on GPU + CudaUniquePtrHost argmax_buf_h_; + // device buffer for argmax postprocessing on GPU + CudaUniquePtr argmax_buf_d_; + std::vector sematic_color_map_; }; } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp index 6044148a932ae..48ffedb98441d 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -15,6 +15,8 @@ #ifndef TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ #define TENSORRT_YOLOX__TENSORRT_YOLOX_NODE_HPP_ +#include "object_recognition_utils/object_recognition_utils.hpp" + #include #include #include @@ -25,6 +27,7 @@ #include #include #include +#include #if __has_include() #include @@ -41,10 +44,30 @@ namespace tensorrt_yolox { +// cspell: ignore Semseg using LabelMap = std::map; - +using Label = tier4_perception_msgs::msg::Semantic; class TrtYoloXNode : public rclcpp::Node { + struct RoiOverlaySemsegLabel + { + bool UNKNOWN; + bool CAR; + bool TRUCK; + bool BUS; + bool MOTORCYCLE; + bool BICYCLE; + bool PEDESTRIAN; + bool ANIMAL; + bool isOverlay(const uint8_t label) const + { + return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) || + (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) || + (label == Label::ANIMAL && ANIMAL) || (label == Label::MOTORBIKE && MOTORCYCLE) || + (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN); + }; + }; // struct RoiOverlaySemsegLabel + public: explicit TrtYoloXNode(const rclcpp::NodeOptions & node_options); @@ -53,8 +76,13 @@ class TrtYoloXNode : public rclcpp::Node void onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg); bool readLabelFile(const std::string & label_path); void replaceLabelMap(); - + void overlapSegmentByRoi( + const tensorrt_yolox::Object & object, cv::Mat & mask, const int width, const int height); + int mapRoiLabel2SegLabel(const int32_t roi_label_index); image_transport::Publisher image_pub_; + image_transport::Publisher mask_pub_; + image_transport::Publisher color_mask_pub_; + rclcpp::Publisher::SharedPtr objects_pub_; image_transport::Subscriber image_sub_; @@ -63,6 +91,21 @@ class TrtYoloXNode : public rclcpp::Node LabelMap label_map_; std::unique_ptr trt_yolox_; + bool is_roi_overlap_segment_; + bool is_publish_color_mask_; + float overlap_roi_score_threshold_; + // TODO(badai-nguyen): change to function + std::map remap_roi_to_semantic_ = { + {"UNKNOWN", 3}, // other + {"ANIMAL", 0}, // other + {"PEDESTRIAN", 6}, // person + {"CAR", 7}, // car + {"TRUCK", 7}, // truck + {"BUS", 7}, // bus + {"BICYCLE", 8}, // bicycle + {"MOTORBIKE", 8}, // motorcycle + }; + RoiOverlaySemsegLabel roi_overlay_segment_labels_; std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; }; diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index dd15eda2913ce..e4436a0424be5 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,22 +1,30 @@ - + - + + + + + diff --git a/perception/tensorrt_yolox/src/preprocess.cu b/perception/tensorrt_yolox/src/preprocess.cu index 3c3087c536f10..f384de2975aa4 100644 --- a/perception/tensorrt_yolox/src/preprocess.cu +++ b/perception/tensorrt_yolox/src/preprocess.cu @@ -594,4 +594,39 @@ void multi_scale_resize_bilinear_letterbox_nhwc_to_nchw32_batch_gpu( cuda_gridsize(N), block, 0, stream>>>(N, dst, src, d_h, d_w, s_h, s_w, d_roi, norm, batch); } +__global__ void argmax_gpu_kernel( + int N, unsigned char * dst, float * src, int dst_h, int dst_w, int src_c, int src_h, int src_w, + int batch) +{ + // NHWC + int index = (blockIdx.x + blockIdx.y * gridDim.x) * blockDim.x + threadIdx.x; + + if (index >= N) return; + int c = 0; + int w = index % dst_w; + int h = index / (dst_w); + + int b; + for (b = 0; b < batch; b++) { + float max_prob = 0.0; + int max_index = 0; + int dst_index = w + dst_w * h + b * dst_h * dst_w; + for (c = 0; c < src_c; c++) { + int src_index = w + src_w * h + c * src_h * src_w + b * src_c * src_h * src_w; + max_index = max_prob < src[src_index] ? c : max_index; + max_prob = max_prob < src[src_index] ? src[src_index] : max_prob; + } + dst[dst_index] = max_index; + } +} + +void argmax_gpu( + unsigned char * dst, float * src, int d_w, int d_h, int s_w, int s_h, int s_c, int batch, + cudaStream_t stream) +{ + int N = d_w * d_h; + argmax_gpu_kernel<<>>( + N, dst, src, d_h, d_w, s_c, s_h, s_w, batch); +} + } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index 11f68f580acc2..34155c3477003 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -15,12 +15,18 @@ #include "cuda_utils/cuda_check_error.hpp" #include "cuda_utils/cuda_unique_ptr.hpp" +#include #include #include #include +#include + #include +#include #include +#include +#include #include #include #include @@ -97,6 +103,52 @@ std::vector loadImageList(const std::string & filename, const std:: } return fileList; } + +std::vector get_seg_colormap(const std::string & filename) +{ + std::vector seg_cmap; + if (filename != "not-specified") { + std::vector color_list = loadListFromTextFile(filename); + for (int i = 0; i < static_cast(color_list.size()); i++) { + if (i == 0) { + // Skip header + continue; + } + std::string colormapString = color_list[i]; + tensorrt_yolox::Colormap cmap; + size_t npos = colormapString.find_first_of(','); + assert(npos != std::string::npos); + std::string substr = colormapString.substr(0, npos); + int id = static_cast(std::stoi(trim(substr))); + colormapString.erase(0, npos + 1); + + npos = colormapString.find_first_of(','); + assert(npos != std::string::npos); + substr = colormapString.substr(0, npos); + std::string name = (trim(substr)); + cmap.id = id; + cmap.name = name; + colormapString.erase(0, npos + 1); + while (!colormapString.empty()) { + size_t npos = colormapString.find_first_of(','); + if (npos != std::string::npos) { + substr = colormapString.substr(0, npos); + unsigned char c = (unsigned char)std::stoi(trim(substr)); + cmap.color.push_back(c); + colormapString.erase(0, npos + 1); + } else { + unsigned char c = (unsigned char)std::stoi(trim(colormapString)); + cmap.color.push_back(c); + break; + } + } + + seg_cmap.push_back(cmap); + } + } + return seg_cmap; +} + } // anonymous namespace namespace tensorrt_yolox @@ -106,12 +158,14 @@ TrtYoloX::TrtYoloX( const float score_threshold, const float nms_threshold, tensorrt_common::BuildConfig build_config, const bool use_gpu_preprocess, std::string calibration_image_list_path, const double norm_factor, [[maybe_unused]] const std::string & cache_dir, const tensorrt_common::BatchConfig & batch_config, - const size_t max_workspace_size) + const size_t max_workspace_size, const std::string & color_map_path) { src_width_ = -1; src_height_ = -1; norm_factor_ = norm_factor; batch_size_ = batch_config[2]; + multitask_ = 0; + sematic_color_map_ = get_seg_colormap(color_map_path); if (precision == "int8") { if (build_config.clip_value <= 0.0) { if (calibration_image_list_path.empty()) { @@ -196,9 +250,18 @@ TrtYoloX::TrtYoloX( needs_output_decode_ = false; break; default: + needs_output_decode_ = true; + // The following three values are considered only if the specified model is plain one + num_class_ = num_class; + score_threshold_ = score_threshold; + nms_threshold_ = nms_threshold; + // Todo : Support multiple segmentation heads + multitask_++; + /* std::stringstream s; s << "\"" << model_path << "\" is unsupported format"; throw std::runtime_error{s.str()}; + */ } // GPU memory allocation @@ -234,10 +297,31 @@ TrtYoloX::TrtYoloX( out_scores_d_ = cuda_utils::make_unique(batch_config[2] * max_detections_); out_classes_d_ = cuda_utils::make_unique(batch_config[2] * max_detections_); } + if (multitask_) { + // Allocate buffer for segmentation + segmentation_out_elem_num_ = 0; + for (int m = 0; m < multitask_; m++) { + const auto output_dims = + trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + size_t out_elem_num = std::accumulate( + output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); + out_elem_num = out_elem_num * batch_config[2]; + segmentation_out_elem_num_ += out_elem_num; + } + segmentation_out_elem_num_per_batch_ = + static_cast(segmentation_out_elem_num_ / batch_config[2]); + segmentation_out_prob_d_ = cuda_utils::make_unique(segmentation_out_elem_num_); + segmentation_out_prob_h_ = + cuda_utils::make_unique_host(segmentation_out_elem_num_, cudaHostAllocPortable); + } if (use_gpu_preprocess) { use_gpu_preprocess_ = true; image_buf_h_ = nullptr; image_buf_d_ = nullptr; + if (multitask_) { + argmax_buf_h_ = nullptr; + argmax_buf_d_ = nullptr; + } } else { use_gpu_preprocess_ = false; } @@ -252,6 +336,9 @@ TrtYoloX::~TrtYoloX() if (image_buf_d_) { image_buf_d_.reset(); } + if (argmax_buf_d_) { + argmax_buf_d_.reset(); + } } } @@ -294,6 +381,26 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) width * height * 3 * batch_size_, cudaHostAllocWriteCombined); image_buf_d_ = cuda_utils::make_unique(width * height * 3 * batch_size_); } + if (multitask_) { + size_t argmax_out_elem_num = 0; + for (int m = 0; m < multitask_; m++) { + const auto output_dims = + trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + const float scale = std::min( + output_dims.d[3] / static_cast(width), + output_dims.d[2] / static_cast(height)); + int out_w = static_cast(width * scale); + int out_h = static_cast(height * scale); + // size_t out_elem_num = std::accumulate( + // output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); + // out_elem_num = out_elem_num * batch_size_; + size_t out_elem_num = out_w * out_h * batch_size_; + argmax_out_elem_num += out_elem_num; + } + argmax_buf_h_ = + cuda_utils::make_unique_host(argmax_out_elem_num, cudaHostAllocPortable); + argmax_buf_d_ = cuda_utils::make_unique(argmax_out_elem_num); + } } } @@ -321,6 +428,12 @@ void TrtYoloX::preprocessGpu(const std::vector & images) if (image_buf_d_) { image_buf_d_.reset(); } + if (argmax_buf_h_) { + argmax_buf_h_.reset(); + } + if (argmax_buf_d_) { + argmax_buf_d_.reset(); + } } } src_width_ = width; @@ -333,6 +446,7 @@ void TrtYoloX::preprocessGpu(const std::vector & images) const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); int b = 0; + size_t argmax_out_elem_num = 0; for (const auto & image : images) { if (!image_buf_h_) { const float scale = std::min(input_width / image.cols, input_height / image.rows); @@ -348,7 +462,31 @@ void TrtYoloX::preprocessGpu(const std::vector & images) image_buf_h_.get() + index, &image.data[0], image.cols * image.rows * 3 * sizeof(unsigned char)); b++; + + if (multitask_) { + for (int m = 0; m < multitask_; m++) { + const auto output_dims = + trt_common_->getBindingDimensions(m + 2); // 0: input, 1: output for detections + const float scale = std::min( + output_dims.d[3] / static_cast(image.cols), + output_dims.d[2] / static_cast(image.rows)); + int out_w = static_cast(image.cols * scale); + int out_h = static_cast(image.rows * scale); + argmax_out_elem_num += out_w * out_h * batch_size; + } + } + } + + if (multitask_) { + if (!argmax_buf_h_) { + argmax_buf_h_ = + cuda_utils::make_unique_host(argmax_out_elem_num, cudaHostAllocPortable); + } + if (!argmax_buf_d_) { + argmax_buf_d_ = cuda_utils::make_unique(argmax_out_elem_num); + } } + // Copy into device memory CHECK_CUDA_ERROR(cudaMemcpyAsync( image_buf_d_.get(), image_buf_h_.get(), @@ -406,7 +544,9 @@ void TrtYoloX::preprocess(const std::vector & images) // No Need for Sync } -bool TrtYoloX::doInference(const std::vector & images, ObjectArrays & objects) +bool TrtYoloX::doInference( + const std::vector & images, ObjectArrays & objects, std::vector & masks, + [[maybe_unused]] std::vector & color_masks) { if (!trt_common_->isInitialized()) { return false; @@ -419,7 +559,7 @@ bool TrtYoloX::doInference(const std::vector & images, ObjectArrays & o } if (needs_output_decode_) { - return feedforwardAndDecode(images, objects); + return feedforwardAndDecode(images, objects, masks, color_masks); } else { return feedforward(images, objects); } @@ -659,6 +799,8 @@ void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & images, ObjectArrays & objects, const std::vector & rois) { + std::vector masks; + std::vector color_masks; if (!trt_common_->isInitialized()) { return false; } @@ -669,7 +811,7 @@ bool TrtYoloX::doInferenceWithRoi( } if (needs_output_decode_) { - return feedforwardAndDecode(images, objects); + return feedforwardAndDecode(images, objects, masks, color_masks); } else { return feedforward(images, objects); } @@ -678,7 +820,6 @@ bool TrtYoloX::doInferenceWithRoi( bool TrtYoloX::doMultiScaleInference( const cv::Mat & image, ObjectArrays & objects, const std::vector & rois) { - std::vector images; if (!trt_common_->isInitialized()) { return false; } @@ -747,10 +888,14 @@ bool TrtYoloX::feedforward(const std::vector & images, ObjectArrays & o return true; } -bool TrtYoloX::feedforwardAndDecode(const std::vector & images, ObjectArrays & objects) +bool TrtYoloX::feedforwardAndDecode( + const std::vector & images, ObjectArrays & objects, std::vector & out_masks, + [[maybe_unused]] std::vector & color_masks) { std::vector buffers = {input_d_.get(), out_prob_d_.get()}; - + if (multitask_) { + buffers = {input_d_.get(), out_prob_d_.get(), segmentation_out_prob_d_.get()}; + } trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); const auto batch_size = images.size(); @@ -758,6 +903,11 @@ bool TrtYoloX::feedforwardAndDecode(const std::vector & images, ObjectA CHECK_CUDA_ERROR(cudaMemcpyAsync( out_prob_h_.get(), out_prob_d_.get(), sizeof(float) * out_elem_num_, cudaMemcpyDeviceToHost, *stream_)); + if (multitask_ && !use_gpu_preprocess_) { + CHECK_CUDA_ERROR(cudaMemcpyAsync( + segmentation_out_prob_h_.get(), segmentation_out_prob_d_.get(), + sizeof(float) * segmentation_out_elem_num_, cudaMemcpyDeviceToHost, *stream_)); + } cudaStreamSynchronize(*stream_); objects.clear(); @@ -766,7 +916,43 @@ bool TrtYoloX::feedforwardAndDecode(const std::vector & images, ObjectA float * batch_prob = out_prob_h_.get() + (i * out_elem_num_per_batch_); ObjectArray object_array; decodeOutputs(batch_prob, object_array, scales_[i], image_size); + // add refine mask using object objects.emplace_back(object_array); + if (multitask_) { + segmentation_masks_.clear(); + + size_t counter = 0; + int batch = + static_cast(segmentation_out_elem_num_ / segmentation_out_elem_num_per_batch_); + for (int m = 0; m < multitask_; m++) { + const auto output_dims = + trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + size_t out_elem_num = std::accumulate( + output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); + out_elem_num = out_elem_num * batch; + const float scale = std::min( + output_dims.d[3] / static_cast(image_size.width), + output_dims.d[2] / static_cast(image_size.height)); + int out_w = static_cast(image_size.width * scale); + int out_h = static_cast(image_size.height * scale); + cv::Mat mask; + if (use_gpu_preprocess_) { + float * d_segmentation_results = + segmentation_out_prob_d_.get() + (i * segmentation_out_elem_num_per_batch_); + mask = getMaskImageGpu(&(d_segmentation_results[counter]), output_dims, out_w, out_h, i); + } else { + float * segmentation_results = + segmentation_out_prob_h_.get() + (i * segmentation_out_elem_num_per_batch_); + mask = getMaskImage(&(segmentation_results[counter]), output_dims, out_w, out_h); + } + segmentation_masks_.emplace_back(std::move(mask)); + counter += out_elem_num; + } + // semantic segmentation was fixed as first task + out_masks.at(i) = segmentation_masks_.at(0); + } else { + continue; + } } return true; } @@ -1036,4 +1222,72 @@ void TrtYoloX::nmsSortedBboxes( } } +cv::Mat TrtYoloX::getMaskImageGpu(float * d_prob, nvinfer1::Dims dims, int out_w, int out_h, int b) +{ + // NCHW + int classes = dims.d[1]; + int height = dims.d[2]; + int width = dims.d[3]; + cv::Mat mask = cv::Mat::zeros(out_h, out_w, CV_8UC1); + int index = b * out_w * out_h; + argmax_gpu( + (unsigned char *)argmax_buf_d_.get() + index, d_prob, out_w, out_h, width, height, classes, 1, + *stream_); + CHECK_CUDA_ERROR(cudaMemcpyAsync( + argmax_buf_h_.get(), argmax_buf_d_.get(), sizeof(unsigned char) * 1 * out_w * out_h, + cudaMemcpyDeviceToHost, *stream_)); + cudaStreamSynchronize(*stream_); + std::memcpy(mask.data, argmax_buf_h_.get() + index, sizeof(unsigned char) * 1 * out_w * out_h); + return mask; +} + +cv::Mat TrtYoloX::getMaskImage(float * prob, nvinfer1::Dims dims, int out_w, int out_h) +{ + // NCHW + int classes = dims.d[1]; + int height = dims.d[2]; + int width = dims.d[3]; + cv::Mat mask = cv::Mat::zeros(out_h, out_w, CV_8UC1); + // argmax + // #pragma omp parallel for + for (int y = 0; y < out_h; y++) { + for (int x = 0; x < out_w; x++) { + float max = 0.0; + int index = 0; + for (int c = 0; c < classes; c++) { + float value = prob[c * height * width + y * width + x]; + if (max < value) { + max = value; + index = c; + } + } + mask.at(y, x) = index; + } + } + return mask; +} + +int TrtYoloX::getMultitaskNum(void) +{ + return multitask_; +} + +void TrtYoloX::getColorizedMask( + const std::vector & colormap, const cv::Mat & mask, cv::Mat & cmask) +{ + int width = mask.cols; + int height = mask.rows; + if ((cmask.cols != mask.cols) || (cmask.rows != mask.rows)) { + throw std::runtime_error("input and output image have difference size."); + } + for (int y = 0; y < height; y++) { + for (int x = 0; x < width; x++) { + unsigned char id = mask.at(y, x); + cmask.at(y, x)[0] = colormap[id].color[2]; + cmask.at(y, x)[1] = colormap[id].color[1]; + cmask.at(y, x)[2] = colormap[id].color[0]; + } + } +} + } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index 4ee18b99e4bc6..c13384918bd9c 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -33,7 +33,7 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = - std::make_unique(this, "tensorrt_yolox"); + std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -96,25 +96,51 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) ("Path to a file which contains path to images." "Those images will be used for int8 quantization.")); + std::string color_map_path = declare_parameter_with_description( + "color_map_path", "", ("Path to a file which contains path to color map.")); if (!readLabelFile(label_path)) { RCLCPP_ERROR(this->get_logger(), "Could not find label file"); rclcpp::shutdown(); } + + is_roi_overlap_segment_ = declare_parameter("is_roi_overlap_segment"); + is_publish_color_mask_ = declare_parameter("is_publish_color_mask"); + overlap_roi_score_threshold_ = declare_parameter("overlap_roi_score_threshold"); + roi_overlay_segment_labels_.UNKNOWN = + declare_parameter("roi_overlay_segment_label.UNKNOWN"); + roi_overlay_segment_labels_.CAR = declare_parameter("roi_overlay_segment_label.CAR"); + roi_overlay_segment_labels_.TRUCK = declare_parameter("roi_overlay_segment_label.TRUCK"); + roi_overlay_segment_labels_.BUS = declare_parameter("roi_overlay_segment_label.BUS"); + roi_overlay_segment_labels_.MOTORCYCLE = + declare_parameter("roi_overlay_segment_label.MOTORCYCLE"); + roi_overlay_segment_labels_.BICYCLE = + declare_parameter("roi_overlay_segment_label.BICYCLE"); + roi_overlay_segment_labels_.PEDESTRIAN = + declare_parameter("roi_overlay_segment_label.PEDESTRIAN"); + roi_overlay_segment_labels_.ANIMAL = declare_parameter("roi_overlay_segment_label.ANIMAL"); replaceLabelMap(); tensorrt_common::BuildConfig build_config( calibration_algorithm, dla_core_id, quantize_first_layer, quantize_last_layer, profile_per_layer, clip_value); + const double norm_factor = 1.0; + const std::string cache_dir = ""; + const tensorrt_common::BatchConfig batch_config{1, 1, 1}; + const size_t max_workspace_size = (1 << 30); + trt_yolox_ = std::make_unique( model_path, precision, label_map_.size(), score_threshold, nms_threshold, build_config, - preprocess_on_gpu, calibration_image_list_path); + preprocess_on_gpu, calibration_image_list_path, norm_factor, cache_dir, batch_config, + max_workspace_size, color_map_path); timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrtYoloXNode::onConnect, this)); objects_pub_ = this->create_publisher( "~/out/objects", 1); + mask_pub_ = image_transport::create_publisher(this, "~/out/mask"); + color_mask_pub_ = image_transport::create_publisher(this, "~/out/color_mask"); image_pub_ = image_transport::create_publisher(this, "~/out/image"); if (declare_parameter("build_only", false)) { @@ -129,7 +155,8 @@ void TrtYoloXNode::onConnect() if ( objects_pub_->get_subscription_count() == 0 && objects_pub_->get_intra_process_subscription_count() == 0 && - image_pub_.getNumSubscribers() == 0) { + image_pub_.getNumSubscribers() == 0 && mask_pub_.getNumSubscribers() == 0 && + color_mask_pub_.getNumSubscribers() == 0) { image_sub_.shutdown(); } else if (!image_sub_) { image_sub_ = image_transport::create_subscription( @@ -154,10 +181,16 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) const auto height = in_image_ptr->image.rows; tensorrt_yolox::ObjectArrays objects; - if (!trt_yolox_->doInference({in_image_ptr->image}, objects)) { + std::vector masks = {cv::Mat(cv::Size(height, width), CV_8UC1, cv::Scalar(0))}; + std::vector color_masks = { + cv::Mat(cv::Size(height, width), CV_8UC3, cv::Scalar(0, 0, 0))}; + + if (!trt_yolox_->doInference({in_image_ptr->image}, objects, masks, color_masks)) { RCLCPP_WARN(this->get_logger(), "Fail to inference"); return; } + auto & mask = masks.at(0); + for (const auto & yolox_object : objects.at(0)) { tier4_perception_msgs::msg::DetectedObjectWithFeature object; object.feature.roi.x_offset = yolox_object.x_offset; @@ -177,9 +210,21 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) cv::rectangle( in_image_ptr->image, cv::Point(left, top), cv::Point(right, bottom), cv::Scalar(0, 0, 255), 3, 8, 0); + // Refine mask: replacing segmentation mask by roi class + // This should remove when the segmentation accuracy is high + if (is_roi_overlap_segment_ && trt_yolox_->getMultitaskNum() > 0) { + overlapSegmentByRoi(yolox_object, mask, width, height); + } + } + // TODO(badai-nguyen): consider to change to 4bits data transfer + if (trt_yolox_->getMultitaskNum() > 0) { + sensor_msgs::msg::Image::SharedPtr out_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::MONO8, mask) + .toImageMsg(); + out_mask_msg->header = msg->header; + mask_pub_.publish(out_mask_msg); } image_pub_.publish(in_image_ptr->toImageMsg()); - out_objects.header = msg->header; objects_pub_->publish(out_objects); @@ -198,6 +243,16 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } + + if (is_publish_color_mask_ && trt_yolox_->getMultitaskNum() > 0) { + cv::Mat color_mask = cv::Mat::zeros(mask.rows, mask.cols, CV_8UC3); + trt_yolox_->getColorizedMask(trt_yolox_->getColorMap(), mask, color_mask); + sensor_msgs::msg::Image::SharedPtr output_color_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::BGR8, color_mask) + .toImageMsg(); + output_color_mask_msg->header = msg->header; + color_mask_pub_.publish(output_color_mask_msg); + } } bool TrtYoloXNode::readLabelFile(const std::string & label_path) @@ -234,6 +289,37 @@ void TrtYoloXNode::replaceLabelMap() } } +int TrtYoloXNode::mapRoiLabel2SegLabel(const int32_t roi_label_index) +{ + if (roi_overlay_segment_labels_.isOverlay(static_cast(roi_label_index))) { + std::string label = label_map_[roi_label_index]; + + return remap_roi_to_semantic_[label]; + } + return -1; +} + +void TrtYoloXNode::overlapSegmentByRoi( + const tensorrt_yolox::Object & roi_object, cv::Mat & mask, const int orig_width, + const int orig_height) +{ + if (roi_object.score < overlap_roi_score_threshold_) return; + int seg_class_index = mapRoiLabel2SegLabel(roi_object.type); + if (seg_class_index < 0) return; + + const float scale_x = static_cast(mask.cols) / static_cast(orig_width); + const float scale_y = static_cast(mask.rows) / static_cast(orig_height); + const int roi_width = static_cast(roi_object.width * scale_x); + const int roi_height = static_cast(roi_object.height * scale_y); + const int roi_x_offset = static_cast(roi_object.x_offset * scale_x); + const int roi_y_offset = static_cast(roi_object.y_offset * scale_y); + + cv::Mat replace_roi( + cv::Size(roi_width, roi_height), mask.type(), static_cast(seg_class_index)); + replace_roi.copyTo(mask.colRange(roi_x_offset, roi_x_offset + roi_width) + .rowRange(roi_y_offset, roi_y_offset + roi_height)); +} + } // namespace tensorrt_yolox #include "rclcpp_components/register_node_macro.hpp" diff --git a/perception/tensorrt_yolox/src/yolox_single_image_inference_node.cpp b/perception/tensorrt_yolox/src/yolox_single_image_inference_node.cpp index 0657f0096b07e..360f41e470e38 100644 --- a/perception/tensorrt_yolox/src/yolox_single_image_inference_node.cpp +++ b/perception/tensorrt_yolox/src/yolox_single_image_inference_node.cpp @@ -47,7 +47,9 @@ class YoloXSingleImageInferenceNode : public rclcpp::Node auto trt_yolox = std::make_unique(model_path, precision); auto image = cv::imread(image_path); tensorrt_yolox::ObjectArrays objects; - trt_yolox->doInference({image}, objects); + std::vector masks; + std::vector color_masks; + trt_yolox->doInference({image}, objects, masks, color_masks); for (const auto & object : objects[0]) { const auto left = object.x_offset; const auto top = object.y_offset; diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index e6e60cd97272b..05b47723d6a0c 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -25,7 +25,12 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif + #include #include #include diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp index 638e2797f0a6e..3cd270adf9383 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp @@ -27,7 +27,12 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif + #include #include #include diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp index c14a5d56dc5d4..066f438a9544f 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp @@ -24,7 +24,12 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif + #include #include #include diff --git a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp index a4d916d968071..f89b8efa35892 100644 --- a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp @@ -23,7 +23,11 @@ #include #include -#include +#if __has_include() +#include // for ROS 2 Jazzy or newer +#else +#include // for ROS 2 Humble or older +#endif #include #include #include diff --git a/planning/.pages b/planning/.pages index 3c868628198a8..2e3dd92011d19 100644 --- a/planning/.pages +++ b/planning/.pages @@ -29,7 +29,7 @@ nav: - 'No Stopping Area': planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module - 'Occlusion Spot': planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module - 'Run Out': planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module - - 'Speed Bump': planning/behavior_velocity_planner/behavior_velocity_speed_bump_module + - 'Speed Bump': planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module - 'Stop Line': planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module - 'Traffic Light': planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module - 'Virtual Traffic Light': planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module @@ -79,7 +79,6 @@ nav: - 'RTC Interface': planning/autoware_rtc_interface - 'Additional Tools': - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator - - 'RTC Replayer': planning/rtc_replayer - 'Planning Debug Tools': - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 0e9a27622fbd9..b3a0fc014c410 100644 --- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -339,16 +339,21 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const auto stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( input_traj_points, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); - if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { - initial_vel = engage_velocity_; - initial_acc = engage_acceleration_; + if (!stop_dist.has_value()) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " + "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f)", + vehicle_speed, target_vel, engage_velocity_, engage_vel_thr); + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else if (stop_dist.value() > stop_dist_to_prohibit_engage_) { RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", vehicle_speed, target_vel, engage_velocity_, engage_vel_thr, stop_dist.value()); - return std::make_tuple(initial_vel, initial_acc); - } else if (stop_dist) { + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else { RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist.value()); diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 345265b1d20b0..d0736c6158a7a 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -509,7 +509,6 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( const double v0, const double a0, const double target_acc, const double target_jerk_ratio) const { // calculate vt function - const auto & i = longitudinal_info_; const auto vt = [&]( const double v0, const double a0, const double jerk, const double t, const double target_acc) { @@ -541,8 +540,8 @@ std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( // Therefore, we calculate v1 (velocity at next step) and use it for initial velocity. const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0; const double a1 = a0; // + jerk * 0.1; - const double jerk = - target_acc > a1 ? i.max_jerk * target_jerk_ratio : i.min_jerk * target_jerk_ratio; + const double jerk = target_acc > a1 ? longitudinal_info_.max_jerk * target_jerk_ratio + : longitudinal_info_.min_jerk * target_jerk_ratio; double t_current = 0.0; std::vector s_vec{0.0}; diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 18e0fc7f5bb03..516bf5579b82c 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -623,9 +623,9 @@ std::vector PlannerInterface::generateSlowDownTrajectory( }(); // insert slow down velocity between slow start and end - for (size_t i = (slow_down_start_idx ? *slow_down_start_idx : 0); i <= *slow_down_end_idx; - ++i) { - auto & traj_point = slow_down_traj_points.at(i); + for (size_t j = (slow_down_start_idx ? *slow_down_start_idx : 0); j <= *slow_down_end_idx; + ++j) { + auto & traj_point = slow_down_traj_points.at(j); traj_point.longitudinal_velocity_mps = std::min(traj_point.longitudinal_velocity_mps, static_cast(stable_slow_down_vel)); } @@ -637,7 +637,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel); slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0); - slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0); + slow_down_debug_multi_array_.data.push_back(*slow_down_end_idx); // add virtual wall if (slow_down_start_idx && slow_down_end_idx) { diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp index 35a2fe3ef45da..9a4081c51be3b 100644 --- a/planning/autoware_route_handler/test/test_route_handler.cpp +++ b/planning/autoware_route_handler/test/test_route_handler.cpp @@ -49,10 +49,11 @@ TEST_F(TestRouteHandler, getGoalLaneId) TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) { - set_route_handler("/test_map/overlap_map.osm"); + set_route_handler("overlap_map.osm"); ASSERT_FALSE(route_handler_->isHandlerReady()); - geometry_msgs::msg::Pose start_pose, goal_pose; + geometry_msgs::msg::Pose start_pose; + geometry_msgs::msg::Pose goal_pose; start_pose.position = autoware::universe_utils::createPoint(3728.870361, 73739.281250, 0); start_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, -0.513117, 0.858319); goal_pose.position = autoware::universe_utils::createPoint(3729.961182, 73727.328125, 0); @@ -79,11 +80,12 @@ TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) { - set_route_handler("/test_map/overlap_map.osm"); - set_test_route("/test_route/overlap_test_route.yaml"); + set_route_handler("overlap_map.osm"); + set_test_route("overlap_test_route.yaml"); ASSERT_TRUE(route_handler_->isHandlerReady()); - geometry_msgs::msg::Pose reference_pose, search_pose; + geometry_msgs::msg::Pose reference_pose; + geometry_msgs::msg::Pose search_pose; lanelet::ConstLanelet reference_lanelet; reference_pose.position = autoware::universe_utils::createPoint(3730.88, 73735.3, 0); @@ -102,71 +104,189 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) ASSERT_EQ(closest_lanelet.id(), 345); found_lanelet = route_handler_->getClosestRouteLaneletFromLanelet( - search_pose, reference_lanelet, &closest_lanelet, 3.0, 1.046); + search_pose, reference_lanelet, &closest_lanelet, dist_threshold, yaw_threshold); ASSERT_TRUE(found_lanelet); ASSERT_EQ(closest_lanelet.id(), 277); } -// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) -// { -// lanelet::ConstLanelet closest_lane; - -// Pose search_pose; - -// search_pose.position = autoware::universe_utils::createPoint(-1.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained7 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); - -// ASSERT_TRUE(closest_lane_obtained7); -// ASSERT_EQ(closest_lane.id(), 4775); - -// search_pose.position = autoware::universe_utils::createPoint(-0.5, 1.75, 0); -// const auto closest_lane_obtained = -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); - -// ASSERT_TRUE(closest_lane_obtained); -// ASSERT_EQ(closest_lane.id(), 4775); - -// search_pose.position = autoware::universe_utils::createPoint(-.01, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained3 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, CheckLaneIsInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4785); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_TRUE(is_lane_in_goal_route_section); +} -// ASSERT_TRUE(closest_lane_obtained3); -// ASSERT_EQ(closest_lane.id(), 4775); +TEST_F(TestRouteHandler, CheckLaneIsNotInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4780); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_FALSE(is_lane_in_goal_route_section); +} -// search_pose.position = autoware::universe_utils::createPoint(0.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained1 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, checkGetLaneletSequence) +{ + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); -// ASSERT_TRUE(closest_lane_obtained1); -// ASSERT_EQ(closest_lane.id(), 4775); + lanelet::ConstLanelet closest_lanelet; + const auto found_closest_lanelet = route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + ASSERT_TRUE(found_closest_lanelet); + ASSERT_EQ(closest_lanelet.id(), 4765ul); + + const auto current_lanes = route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + + ASSERT_EQ(current_lanes.size(), 6ul); + ASSERT_EQ(current_lanes.at(0).id(), 4765ul); + ASSERT_EQ(current_lanes.at(1).id(), 4770ul); + ASSERT_EQ(current_lanes.at(2).id(), 4775ul); + ASSERT_EQ(current_lanes.at(3).id(), 4424ul); + ASSERT_EQ(current_lanes.at(4).id(), 4780ul); + ASSERT_EQ(current_lanes.at(5).id(), 4785ul); +} -// search_pose.position = autoware::universe_utils::createPoint(0.01, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained2 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneWhenLaneChangeToRight) +{ + const auto current_lanes = get_current_lanes(); + + // The input is within expectation. + // this lane is of preferred lane type + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // this alternative lane is a subset of preferred lane route section + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); + + // The input is within expectation. + // Although Direction::NONE, the function should still return result similar to + // Direction::RIGHT. + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // Although Direction::NONE is provided, the function should behave similarly to + // Direction::RIGHT. + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); +} -// ASSERT_TRUE(closest_lane_obtained2); -// ASSERT_EQ(closest_lane.id(), 4424); +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneUsingUnexpectedResults) +{ + const auto current_lanes = get_current_lanes(); -// search_pose.position = autoware::universe_utils::createPoint(0.5, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained4 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::LEFT); + ASSERT_EQ(result.size(), 0ul); + }); +} -// ASSERT_TRUE(closest_lane_obtained4); -// ASSERT_EQ(closest_lane.id(), 4424); +TEST_F(TestRouteHandler, testGetCenterLinePath) +{ + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + { + // The input is within expectation. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 50.0); + ASSERT_EQ(center_line_path.points.size(), 51); // 26 + 26 - 1(overlapped) + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 2); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4780); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(1), 4785); + } + { + // The input is broken. + // s_start is negative, and s_end is over the boundary. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, -1.0, 200.0); + ASSERT_EQ(center_line_path.points.size(), 76); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4785); + } +} +TEST_F(TestRouteHandler, DISABLED_testGetCenterLinePathWhenLanesIsNotConnected) +{ + // broken current lanes. 4424 and 4785 are not connected directly. + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + + // The input is broken. Test is disabled because it doesn't pass. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 75.0); + ASSERT_EQ(center_line_path.points.size(), 26); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4424); +} -// search_pose.position = autoware::universe_utils::createPoint(1.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained5 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) +{ + auto get_closest_lanelet_within_route = + [&](double x, double y, double z) -> std::optional { + const auto pose = autoware::test_utils::createPose(x, y, z, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + const auto closest_lane_obtained = + route_handler_->getClosestLaneletWithinRoute(pose, &closest_lanelet); + if (!closest_lane_obtained) { + return std::nullopt; + } + return closest_lanelet.id(); + }; + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.5, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.01, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.0, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.0, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.01, 1.75, 0).value(), 4424ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.5, 1.75, 0).value(), 4424ul); +} -// ASSERT_TRUE(closest_lane_obtained5); -// ASSERT_EQ(closest_lane.id(), 4424); -// } +TEST_F(TestRouteHandler, testGetLaneChangeTargetLanes) +{ + { + // The input is within expectation. + // There exist no lane changing lane since both 4770 and 4775 are preferred lane. + const auto current_lanes = route_handler_->getLaneletsFromIds({4770, 4775}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + ASSERT_FALSE(lane_change_lane.has_value()); + } + + { + // The input is within expectation. + // There exist lane changing lane since 4424 is subset of preferred lane 9598. + const auto current_lanes = route_handler_->getLaneletsFromIds({4775, 4424}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + EXPECT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } + + { + // The input is within expectation. + // There is a lane-changing lane. Within the maximum current lanes, there is an alternative lane + // to the preferred lane. Therefore, the lane-changing lane exists. + const auto current_lanes = get_current_lanes(); + const auto lane_change_lane = route_handler_->getLaneChangeTarget(current_lanes); + ASSERT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } +} } // namespace autoware::route_handler::test diff --git a/planning/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp index 86f7461fc7538..ec3368809df99 100644 --- a/planning/autoware_route_handler/test/test_route_handler.hpp +++ b/planning/autoware_route_handler/test/test_route_handler.hpp @@ -15,7 +15,6 @@ #ifndef TEST_ROUTE_HANDLER_HPP_ #define TEST_ROUTE_HANDLER_HPP_ -#include "ament_index_cpp/get_package_share_directory.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_test_utils/mock_data_parser.hpp" #include "gtest/gtest.h" @@ -37,16 +36,16 @@ namespace autoware::route_handler::test { +using autoware::test_utils::get_absolute_path_to_lanelet_map; +using autoware::test_utils::get_absolute_path_to_route; using autoware_map_msgs::msg::LaneletMapBin; - class TestRouteHandler : public ::testing::Test { public: TestRouteHandler() { - autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); - set_route_handler("/test_map/2km_test.osm"); - set_test_route("/test_route/lane_change_test_route.yaml"); + set_route_handler("2km_test.osm"); + set_test_route(lane_change_right_test_route_filename); } TestRouteHandler(const TestRouteHandler &) = delete; @@ -55,26 +54,44 @@ class TestRouteHandler : public ::testing::Test TestRouteHandler & operator=(TestRouteHandler &&) = delete; ~TestRouteHandler() override = default; - void set_route_handler(const std::string & relative_path) + void set_route_handler(const std::string & lanelet_map_filename) { route_handler_.reset(); - const auto lanelet2_path = autoware_test_utils_dir + relative_path; + const auto lanelet2_path = + get_absolute_path_to_lanelet_map(autoware_test_utils_dir, lanelet_map_filename); const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); route_handler_ = std::make_shared(map_bin_msg); } - void set_test_route(const std::string & route_path) + void set_test_route(const std::string & route_filename) { - const auto route_handler_dir = - ament_index_cpp::get_package_share_directory("autoware_route_handler"); - const auto rh_test_route = route_handler_dir + route_path; + const auto rh_test_route = + get_absolute_path_to_route(autoware_route_handler_dir, route_filename); route_handler_->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); } + lanelet::ConstLanelets get_current_lanes() + { + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + [[maybe_unused]] const auto found_closest_lanelet = + route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + return route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + } + std::shared_ptr route_handler_; - std::string autoware_test_utils_dir; - static constexpr double center_line_resolution = 5.0; + std::string autoware_test_utils_dir{"autoware_test_utils"}; + std::string autoware_route_handler_dir{"autoware_route_handler"}; + std::string lane_change_right_test_route_filename{"lane_change_test_route.yaml"}; + + static constexpr double center_line_resolution{5.0}; + static constexpr double dist_threshold{3.0}; + static constexpr double yaw_threshold{1.045}; + static constexpr double backward_path_length{5.0}; + static constexpr double forward_path_length{300.0}; }; } // namespace autoware::route_handler::test diff --git a/planning/autoware_rtc_interface/README.md b/planning/autoware_rtc_interface/README.md index 19d200592bcf3..635f692266f7e 100644 --- a/planning/autoware_rtc_interface/README.md +++ b/planning/autoware_rtc_interface/README.md @@ -185,6 +185,10 @@ Return `true` if `uuid` is registered. If `uuid` is registered, return `true`. If not, return `false`. +## Debugging Tools + +There is a debugging tool called [RTC replayer](https://github.com/autowarefoundation/autoware_tools/tree/main/planning/autoware_rtc_replayer) for the RTC interface. + ## Assumptions / Known limits ## Future extensions / Unimplemented parts diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt index 08a97c9010008..261e8beb0022f 100644 --- a/planning/autoware_static_centerline_generator/CMakeLists.txt +++ b/planning/autoware_static_centerline_generator/CMakeLists.txt @@ -60,5 +60,7 @@ endif() install(PROGRAMS scripts/app.py + scripts/centerline_updater_helper.py + scripts/show_lanelet2_map_diff.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml index 24a5536949479..060590803428a 100644 --- a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml +++ b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml @@ -1,5 +1,9 @@ /**: ros__parameters: - marker_color: ["FF0000", "00FF00", "0000FF"] + marker_color: ["FF0000", "FF5A00", "FFFF00"] marker_color_dist_thresh : [0.1, 0.2, 0.3] output_trajectory_interval: 1.0 + + validation: + dist_threshold_to_road_border: 0.0 + max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease. diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 3d786ab995e5b..38379a52e4c92 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -3,17 +3,19 @@ - + - - + - + + + + @@ -28,7 +30,7 @@ default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml" /> - + @@ -55,12 +57,8 @@ - - - - - + @@ -75,12 +73,19 @@ + + + + + + + - + diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz index 62b4c9b75ec87..fc916f188e758 100644 --- a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz +++ b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz @@ -179,11 +179,11 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_generator/output_centerline + Value: /static_centerline_generator/output/centerline Value: true View Footprint: - Alpha: 1 - Color: 255; 0; 0 + Alpha: 0.5 + Color: 0; 255; 0 Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: true @@ -268,9 +268,33 @@ Visualization Manager: Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_generator/debug/unsafe_footprints + Value: /static_centerline_generator/output/validation_results Value: true - Enabled: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Debug Markers + Namespaces: + curvature: false + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/debug/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /static_centerline_generator/debug/ego_footprint_bounds + Value: true + Enabled: true Name: debug Enabled: true Global Options: diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py index 3672165caed85..2cd0c2f66d474 100755 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py @@ -20,18 +20,20 @@ from PyQt5 import QtCore from PyQt5.QtWidgets import QApplication -from PyQt5.QtWidgets import QGridLayout +from PyQt5.QtWidgets import QGroupBox from PyQt5.QtWidgets import QMainWindow from PyQt5.QtWidgets import QPushButton from PyQt5.QtWidgets import QSizePolicy from PyQt5.QtWidgets import QSlider +from PyQt5.QtWidgets import QVBoxLayout from PyQt5.QtWidgets import QWidget from autoware_planning_msgs.msg import Trajectory import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSProfile -from std_msgs.msg import Bool +from std_msgs.msg import Empty +from std_msgs.msg import Float32 from std_msgs.msg import Int32 @@ -46,25 +48,8 @@ def setupUI(self): self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) central_widget = QWidget(self) - central_widget.setObjectName("central_widget") - - self.grid_layout = QGridLayout(central_widget) + self.grid_layout = QVBoxLayout(central_widget) self.grid_layout.setContentsMargins(10, 10, 10, 10) - self.grid_layout.setObjectName("grid_layout") - - # button to update the trajectory's start and end index - self.update_button = QPushButton("update slider") - self.update_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.update_button.clicked.connect(self.onUpdateButton) - - # button to reset the trajectory's start and end index - self.reset_button = QPushButton("reset") - self.reset_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.reset_button.clicked.connect(self.onResetButton) - - # button to save map - self.save_map_button = QPushButton("save map") - self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) # slide of the trajectory's start and end index self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal) @@ -72,69 +57,60 @@ def setupUI(self): self.min_traj_start_index = 0 self.max_traj_end_index = None - # set layout - self.grid_layout.addWidget(self.update_button, 1, 0, 1, -1) - self.grid_layout.addWidget(self.reset_button, 2, 0, 1, -1) - self.grid_layout.addWidget(self.save_map_button, 3, 0, 1, -1) - self.grid_layout.addWidget(self.traj_start_index_slider, 4, 0, 1, -1) - self.grid_layout.addWidget(self.traj_end_index_slider, 5, 0, 1, -1) - self.setCentralWidget(central_widget) - - def initWithEndIndex(self, max_traj_end_index): - self.max_traj_end_index = max_traj_end_index - - # initialize slider - self.displayed_min_traj_start_index = self.min_traj_start_index - self.displayed_max_traj_end_index = self.max_traj_end_index - self.initializeSlider() - - def initializeSlider(self, move_value_to_end=True): - self.traj_start_index_slider.setMinimum(0) - self.traj_end_index_slider.setMinimum(0) - self.traj_start_index_slider.setMaximum( - self.displayed_max_traj_end_index - self.displayed_min_traj_start_index - ) - self.traj_end_index_slider.setMaximum( - self.displayed_max_traj_end_index - self.displayed_min_traj_start_index + # Layout: Range of Centerline + centerline_vertical_box = QVBoxLayout(self) + centerline_vertical_box.addWidget(self.traj_start_index_slider) + centerline_vertical_box.addWidget(self.traj_end_index_slider) + centerline_group = QGroupBox("Centerline") + centerline_group.setLayout(centerline_vertical_box) + self.grid_layout.addWidget(centerline_group) + + # 2. Road Boundary + road_boundary_group = QGroupBox("Road Boundary") + road_boundary_vertical_box = QVBoxLayout(self) + road_boundary_group.setLayout(road_boundary_vertical_box) + self.grid_layout.addWidget(road_boundary_group) + + # 2.1. Slider + self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal) + road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider) + self.road_boundary_lateral_margin_ratio = 10 + self.road_boundary_lateral_margin_slider.setMinimum(0) + self.road_boundary_lateral_margin_slider.setMaximum( + 5 * self.road_boundary_lateral_margin_ratio ) - if move_value_to_end: - self.traj_start_index_slider.setValue(0) - self.traj_end_index_slider.setValue(self.traj_end_index_slider.maximum()) + road_boundary_vertical_box.addWidget(QPushButton("reset")) - def onResetButton(self, event): - current_traj_start_index = self.displayed_min_traj_start_index - current_traj_end_index = self.displayed_max_traj_end_index + # 3. General + general_group = QGroupBox("General") + general_vertical_box = QVBoxLayout(self) + general_group.setLayout(general_vertical_box) + self.grid_layout.addWidget(general_group) - self.displayed_min_traj_start_index = self.min_traj_start_index - self.displayed_max_traj_end_index = self.max_traj_end_index + # 3.1. Validate Centerline + self.validate_button = QPushButton("validate") + self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + general_vertical_box.addWidget(self.validate_button) - self.initializeSlider(False) - self.traj_start_index_slider.setValue(current_traj_start_index) - if ( - current_traj_start_index == self.min_traj_start_index - and current_traj_end_index == self.max_traj_end_index - ): - self.traj_end_index_slider.setValue(self.displayed_max_traj_end_index) - else: - self.traj_end_index_slider.setValue( - current_traj_start_index + self.traj_end_index_slider.value() - ) - - def onUpdateButton(self, event): - current_traj_start_index = self.getCurrentStartIndex() - current_traj_end_index = self.getCurrentEndIndex() + # 3.2. Save Map + self.save_map_button = QPushButton("save map") + self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + general_vertical_box.addWidget(self.save_map_button) - self.displayed_min_traj_start_index = current_traj_start_index - self.displayed_max_traj_end_index = current_traj_end_index + self.setCentralWidget(central_widget) - self.initializeSlider() + def initWithEndIndex(self, max_traj_end_index): + self.max_traj_end_index = max_traj_end_index - def getCurrentStartIndex(self): - return self.displayed_min_traj_start_index + self.traj_start_index_slider.value() + # initialize sliders + self.traj_start_index_slider.setMinimum(self.min_traj_start_index) + self.traj_start_index_slider.setMaximum(self.max_traj_end_index) + self.traj_start_index_slider.setValue(self.min_traj_start_index) - def getCurrentEndIndex(self): - return self.displayed_min_traj_start_index + self.traj_end_index_slider.value() + self.traj_end_index_slider.setMinimum(self.min_traj_start_index) + self.traj_end_index_slider.setMaximum(self.max_traj_end_index) + self.traj_end_index_slider.setValue(self.max_traj_end_index) class CenterlineUpdaterHelper(Node): @@ -144,18 +120,30 @@ def __init__(self): self.widget = CenterlineUpdaterWidget() self.widget.show() self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed) + self.widget.validate_button.clicked.connect(self.onValidateButtonPushed) self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged) self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged) + self.widget.road_boundary_lateral_margin_slider.valueChanged.connect( + self.onRoadBoundaryLateralMargin + ) # ROS - self.pub_save_map = self.create_publisher(Bool, "~/save_map", 1) - self.pub_traj_start_index = self.create_publisher(Int32, "~/traj_start_index", 1) - self.pub_traj_end_index = self.create_publisher(Int32, "~/traj_end_index", 1) + self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1) + self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1) + self.pub_traj_start_index = self.create_publisher( + Int32, "/static_centerline_generator/traj_start_index", 1 + ) + self.pub_traj_end_index = self.create_publisher( + Int32, "/static_centerline_generator/traj_end_index", 1 + ) + self.pub_road_boundary_lateral_margin = self.create_publisher( + Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1 + ) transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) self.sub_whole_centerline = self.create_subscription( Trajectory, - "/static_centerline_generator/output_whole_centerline", + "/static_centerline_generator/output/whole_centerline", self.onWholeCenterline, transient_local_qos, ) @@ -168,20 +156,31 @@ def onWholeCenterline(self, whole_centerline): self.widget.initWithEndIndex(len(whole_centerline.points) - 1) def onSaveMapButtonPushed(self, event): - msg = Bool() - msg.data = True + msg = Empty() self.pub_save_map.publish(msg) + def onValidateButtonPushed(self, event): + msg = Empty() + self.pub_validate.publish(msg) + def onStartIndexChanged(self, event): msg = Int32() - msg.data = self.widget.getCurrentStartIndex() + msg.data = self.widget.traj_start_index_slider.value() self.pub_traj_start_index.publish(msg) def onEndIndexChanged(self, event): msg = Int32() - msg.data = self.widget.getCurrentEndIndex() + msg.data = self.widget.traj_end_index_slider.value() self.pub_traj_end_index.publish(msg) + def onRoadBoundaryLateralMargin(self, event): + msg = Float32() + msg.data = ( + self.widget.road_boundary_lateral_margin_slider.value() + / self.widget.road_boundary_lateral_margin_ratio + ) + self.pub_road_boundary_lateral_margin.publish(msg) + def main(args=None): app = QApplication(sys.argv) diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh index e7f86062a9d20..c170e24e475d9 100755 --- a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh +++ b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh @@ -1,3 +1,5 @@ #!/bin/bash -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus + +# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp index 9f52f271cd5e5..5afc7e58ba52b 100644 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ b/planning/autoware_static_centerline_generator/src/main.cpp @@ -25,11 +25,19 @@ int main(int argc, char * argv[]) node_options); // get ros parameter - const bool run_background = node->declare_parameter("run_background"); + const auto mode = node->declare_parameter("mode"); // process - if (!run_background) { - node->run(); + if (mode == "AUTO") { + node->generate_centerline(); + node->validate(); + node->save_map(); + } else if (mode == "GUI") { + node->generate_centerline(); + } else if (mode == "VMB") { + // Do nothing + } else { + throw std::invalid_argument("The `mode` is invalid."); } // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index c5ecc48ee209d..24ccd7d660edf 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -20,6 +20,7 @@ #include "autoware/universe_utils/ros/parameter.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -34,7 +35,7 @@ #include #include -#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/empty.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" @@ -55,6 +56,10 @@ #include #include +#define RESET_TEXT "\x1B[0m" +#define RED_TEXT "\x1B[31m" +#define BOLD_TEXT "\x1B[1m" + namespace autoware::static_centerline_generator { namespace @@ -115,7 +120,7 @@ geometry_msgs::msg::Pose get_text_pose( const auto & i = vehicle_info; const double x_front = i.front_overhang_m + i.wheel_base_m; - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 1.0; + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 0.5; return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); } @@ -171,6 +176,32 @@ std::vector resample_trajectory_points( autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); } + +bool arePointsClose( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const double epsilon) +{ + return std::abs(p1.x - p2.x) < epsilon && std::abs(p1.y - p2.y) < epsilon; +} + +bool areSameDirection( + const double yaw, const geometry_msgs::msg::Point & start_point, + const geometry_msgs::msg::Point & end_point) +{ + return autoware::universe_utils::normalizeRadian( + yaw - std::atan2(end_point.y - start_point.y, end_point.x - start_point.x)) < M_PI_2; +} + +std::vector convertToGeometryPoints(const LineString2d & lanelet_points) +{ + std::vector points; + for (const auto & lanelet_point : lanelet_points) { + geometry_msgs::msg::Point point; + point.x = lanelet_point.x(); + point.y = lanelet_point.y(); + points.push_back(point); + } + return points; +} } // namespace StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( @@ -181,46 +212,53 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( pub_map_bin_ = create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); pub_whole_centerline_ = - create_publisher("output_whole_centerline", utils::create_transient_local_qos()); + create_publisher("~/output/whole_centerline", utils::create_transient_local_qos()); pub_centerline_ = - create_publisher("output_centerline", utils::create_transient_local_qos()); + create_publisher("~/output/centerline", utils::create_transient_local_qos()); // debug publishers - pub_debug_unsafe_footprints_ = - create_publisher("debug/unsafe_footprints", utils::create_transient_local_qos()); + pub_validation_results_ = + create_publisher("~/validation_results", utils::create_transient_local_qos()); + pub_debug_markers_ = + create_publisher("~/debug/markers", utils::create_transient_local_qos()); + + pub_debug_ego_footprint_bounds_ = create_publisher( + "~/debug/ego_footprint_bounds", utils::create_transient_local_qos()); // subscribers + sub_footprint_margin_for_road_bound_ = create_subscription( + "/static_centerline_generator/road_boundary_lateral_margin", rclcpp::QoS{1}, + [this](const std_msgs::msg::Float32 & msg) { footprint_margin_for_road_bound_ = msg.data; }); sub_traj_start_index_ = create_subscription( - "/centerline_updater_helper/traj_start_index", rclcpp::QoS{1}, + "/static_centerline_generator/traj_start_index", rclcpp::QoS{1}, [this](const std_msgs::msg::Int32 & msg) { - update_centerline_range(msg.data, traj_range_indices_.second); + if (centerline_handler_.update_start_index(msg.data)) { + visualize_selected_centerline(); + } }); sub_traj_end_index_ = create_subscription( - "/centerline_updater_helper/traj_end_index", rclcpp::QoS{1}, + "/static_centerline_generator/traj_end_index", rclcpp::QoS{1}, [this](const std_msgs::msg::Int32 & msg) { - update_centerline_range(traj_range_indices_.first, msg.data); + if (centerline_handler_.update_end_index(msg.data)) { + visualize_selected_centerline(); + } }); - sub_save_map_ = create_subscription( - "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { - const auto lanelet2_output_file_path = - autoware::universe_utils::getOrDeclareParameter( - *this, "lanelet2_output_file_path"); - if (!centerline_with_route_ || msg.data) { - const auto & c = *centerline_with_route_; - const auto selected_centerline = std::vector( - c.centerline.begin() + traj_range_indices_.first, - c.centerline.begin() + traj_range_indices_.second + 1); - const auto selected_route_lane_ids = get_route_lane_ids_from_points(selected_centerline); - save_map( - lanelet2_output_file_path, - CenterlineWithRoute{selected_centerline, selected_route_lane_ids}); + sub_save_map_ = create_subscription( + "/static_centerline_generator/save_map", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { + if (!centerline_handler_.is_valid()) { + return; } + save_map(); }); sub_traj_resample_interval_ = create_subscription( - "/centerline_updater_helper/traj_resample_interval", rclcpp::QoS{1}, + "/static_centerline_generator/traj_resample_interval", rclcpp::QoS{1}, [this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) { // TODO(murooka) }); + sub_validate_ = create_subscription( + "/static_centerline_generator/validate", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); }); // services callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -259,43 +297,43 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( }(); } -void StaticCenterlineGeneratorNode::update_centerline_range( - const int traj_start_index, const int traj_end_index) +void StaticCenterlineGeneratorNode::visualize_selected_centerline() { - if (!centerline_with_route_ || traj_range_indices_.second + 1 < traj_start_index) { - return; - } - - traj_range_indices_ = std::make_pair(traj_start_index, traj_end_index); - - const auto & centerline = centerline_with_route_->centerline; - const auto selected_centerline = std::vector( - centerline.begin() + traj_range_indices_.first, - centerline.begin() + traj_range_indices_.second + 1); - + // publish selected centerline + const auto selected_centerline = centerline_handler_.get_selected_centerline(); pub_centerline_->publish( autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); + + // delete markers for validation + pub_validation_results_->publish(utils::create_delete_all_marker_array({}, now())); + pub_debug_markers_->publish(utils::create_delete_all_marker_array( + {"unsafe_footprints", "unsafe_footprints_distance"}, now())); + pub_debug_ego_footprint_bounds_->publish( + utils::create_delete_all_marker_array({"road_bounds"}, now())); } -void StaticCenterlineGeneratorNode::run() +void StaticCenterlineGeneratorNode::generate_centerline() { // declare planning setting parameters const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); - const auto lanelet2_output_file_path = - declare_parameter("lanelet2_output_file_path"); // process load_map(lanelet2_input_file_path); - const auto centerline_with_route = generate_centerline_with_route(); - traj_range_indices_ = std::make_pair(0, centerline_with_route.centerline.size() - 1); + const auto whole_centerline_with_route = generate_whole_centerline_with_route(); + centerline_handler_ = CenterlineHandler(whole_centerline_with_route); - evaluate(centerline_with_route.route_lane_ids, centerline_with_route.centerline); - save_map(lanelet2_output_file_path, centerline_with_route); + visualize_selected_centerline(); +} - centerline_with_route_ = centerline_with_route; +void StaticCenterlineGeneratorNode::validate() +{ + const auto selected_centerline = centerline_handler_.get_selected_centerline(); + const auto road_bounds = update_road_boundary(selected_centerline); + + evaluate(); } -CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_route() +CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route() { if (!route_handler_ptr_) { RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); @@ -307,14 +345,15 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); const auto optimized_centerline = optimization_trajectory_based_centerline_.generate_centerline_with_optimization( *this, *route_handler_ptr_, route_lane_ids); return CenterlineWithRoute{optimized_centerline, route_lane_ids}; } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { const auto bag_centerline = generate_centerline_with_bag(*this); - const auto route_lane_ids = get_route_lane_ids_from_points(bag_centerline); + const auto route_lane_ids = + plan_route(bag_centerline.front().pose, bag_centerline.back().pose); return CenterlineWithRoute{bag_centerline, route_lane_ids}; } throw std::logic_error( @@ -329,30 +368,9 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); - pub_centerline_->publish(autoware::motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); - return centerline_with_route; } -std::vector StaticCenterlineGeneratorNode::get_route_lane_ids_from_points( - const std::vector & points) -{ - const auto start_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.front().pose); - const auto end_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.back().pose); - if (start_lanelets.empty() || end_lanelets.empty()) { - RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's points are not found."); - return std::vector{}; - } - - const lanelet::Id start_lanelet_id = start_lanelets.front().id(); - const lanelet::Id end_lanelet_id = end_lanelets.front().id(); - if (start_lanelet_id == end_lanelet_id) { - return std::vector{start_lanelet_id}; - } - return plan_route(start_lanelet_id, end_lanelet_id); -} - void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { // copy the input LL2 map to the temporary file for debugging @@ -426,24 +444,35 @@ void StaticCenterlineGeneratorNode::on_load_map( response->message = "InvalidMapFormat"; } -std::vector StaticCenterlineGeneratorNode::plan_route( +std::vector StaticCenterlineGeneratorNode::plan_route_by_lane_ids( const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) { - if (!map_bin_ptr_ || !route_handler_ptr_) { + if (!route_handler_ptr_) { RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); return std::vector{}; } - // calculate check points (= start and goal pose) - const auto check_points = [&]() { - const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); - const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); - return std::vector{start_center_pose, end_center_pose}; - }(); - RCLCPP_INFO(get_logger(), "Calculated check points."); + const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); + const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); + return plan_route(start_center_pose, end_center_pose); +} + +std::vector StaticCenterlineGeneratorNode::plan_route( + const geometry_msgs::msg::Pose & start_center_pose, + const geometry_msgs::msg::Pose & end_center_pose) +{ + if (!map_bin_ptr_) { + RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); + return std::vector{}; + } // plan route by the mission_planner package const auto route = [&]() { + // calculate check points + RCLCPP_INFO(get_logger(), "Calculated check points."); + const auto check_points = + std::vector{start_center_pose, end_center_pose}; + // create mission_planner plugin auto plugin_loader = pluginlib::ClassLoader( "autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"); @@ -459,10 +488,16 @@ std::vector StaticCenterlineGeneratorNode::plan_route( return route; }(); - RCLCPP_INFO(get_logger(), "Planned route."); // get lanelets const auto route_lane_ids = get_lane_ids_from_route(route); + + std::string route_lane_ids_str = ""; + for (const lanelet::Id route_lane_id : route_lane_ids) { + route_lane_ids_str += std::to_string(route_lane_id) + ","; + } + RCLCPP_INFO_STREAM(get_logger(), "Planned route. (" << route_lane_ids_str << ")"); + return route_lane_ids; } @@ -479,7 +514,7 @@ void StaticCenterlineGeneratorNode::on_plan_route( const lanelet::Id end_lanelet_id = request->end_lane_id; // plan route - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // extract lane ids @@ -533,8 +568,11 @@ void StaticCenterlineGeneratorNode::on_plan_path( return; } + centerline_handler_ = + CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids}); + // publish unsafe_footprints - evaluate(route_lane_ids, optimized_traj_points); + evaluate(); // create output data auto target_traj_point = optimized_traj_points.cbegin(); @@ -572,16 +610,128 @@ void StaticCenterlineGeneratorNode::on_plan_path( response->message = ""; } -void StaticCenterlineGeneratorNode::evaluate( - const std::vector & route_lane_ids, - const std::vector & optimized_traj_points) +RoadBounds StaticCenterlineGeneratorNode::update_road_boundary( + const std::vector & centerline) +{ + const double max_ego_lon_offset = vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m; + const double min_ego_lon_offset = -vehicle_info_.rear_overhang_m; + const double max_ego_lat_offset = + vehicle_info_.wheel_tread_m / 2.0 + vehicle_info_.left_overhang_m; + const double ego_lat_offset = max_ego_lat_offset + footprint_margin_for_road_bound_; + + std::vector ego_left_bound; + std::vector ego_right_bound; + for (size_t i = 0; i < centerline.size(); ++i) { + const auto & centerline_point = centerline.at(i).pose; + if (i == 0) { + // Add the first bound point + ego_left_bound.push_back(autoware::universe_utils::calcOffsetPose( + centerline_point, min_ego_lon_offset, ego_lat_offset, 0.0) + .position); + ego_right_bound.push_back(autoware::universe_utils::calcOffsetPose( + centerline_point, min_ego_lon_offset, -ego_lat_offset, 0.0) + .position); + } + + if (i == centerline.size() - 1) { + // Add the last bound point + const auto ego_left_bound_last_point = + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, ego_lat_offset, 0.0) + .position; + if (!arePointsClose(ego_left_bound.back(), ego_left_bound_last_point, 1e-6)) { + ego_left_bound.push_back(ego_left_bound_last_point); + } + const auto ego_right_bound_last_point = + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, -ego_lat_offset, 0.0) + .position; + if (!arePointsClose(ego_right_bound.back(), ego_right_bound_last_point, 1e-6)) { + ego_right_bound.push_back(ego_right_bound_last_point); + } + } else { + // Calculate new bound point depending on the orientation + const auto & next_centerline_point = centerline.at(i + 1).pose; + const double diff_yaw = autoware::universe_utils::normalizeRadian( + tf2::getYaw(next_centerline_point.orientation) - tf2::getYaw(centerline_point.orientation)); + const auto [ego_left_bound_new_point, ego_right_bound_new_point] = [&]() { + if (0 < diff_yaw) { + return std::make_pair( + autoware::universe_utils::calcOffsetPose(centerline_point, 0.0, ego_lat_offset, 0.0) + .position, + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, -ego_lat_offset, 0.0) + .position); + } + return std::make_pair( + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, ego_lat_offset, 0.0) + .position, + autoware::universe_utils::calcOffsetPose(centerline_point, 0.0, -ego_lat_offset, 0.0) + .position); + }(); + + // Check if the bound will be longitudinally monotonic. + if (areSameDirection( + tf2::getYaw(centerline_point.orientation), ego_left_bound.back(), + ego_left_bound_new_point)) { + ego_left_bound.push_back(ego_left_bound_new_point); + } + if (areSameDirection( + tf2::getYaw(centerline_point.orientation), ego_right_bound.back(), + ego_right_bound_new_point)) { + ego_right_bound.push_back(ego_right_bound_new_point); + } + } + } + + // Publish marker + MarkerArray ego_footprint_bounds_marker_array; + { + auto left_bound_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "road_bounds", 0, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.7, 0.8)); + left_bound_marker.lifetime = rclcpp::Duration(0, 0); + for (const auto & ego_left_bound_point : ego_left_bound) { + left_bound_marker.points.push_back(ego_left_bound_point); + } + ego_footprint_bounds_marker_array.markers.push_back(left_bound_marker); + } + { + auto right_bound_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "road_bounds", 1, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.7, 0.8)); + right_bound_marker.lifetime = rclcpp::Duration(0, 0); + for (const auto & ego_right_bound_point : ego_right_bound) { + right_bound_marker.points.push_back(ego_right_bound_point); + } + ego_footprint_bounds_marker_array.markers.push_back(right_bound_marker); + } + pub_debug_ego_footprint_bounds_->publish(ego_footprint_bounds_marker_array); + + return RoadBounds{ego_left_bound, ego_right_bound}; +} + +void StaticCenterlineGeneratorNode::evaluate() { + std::cerr << std::endl + << "############################################## Validation Results " + "##############################################" + << std::endl; + + const auto & centerline = centerline_handler_.get_selected_centerline(); + const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - const auto dist_thresh_vec = autoware::universe_utils::getOrDeclareParameter>( - *this, "marker_color_dist_thresh"); - const auto marker_color_vec = - autoware::universe_utils::getOrDeclareParameter>( - *this, "marker_color"); + + const double dist_thresh_to_road_border = + getRosParameter("validation.dist_threshold_to_road_border"); + const double max_steer_angle_margin = + getRosParameter("validation.max_steer_angle_margin"); + + const auto dist_thresh_vec = getRosParameter>("marker_color_dist_thresh"); + const auto marker_color_vec = getRosParameter>("marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); @@ -593,27 +743,34 @@ void StaticCenterlineGeneratorNode::evaluate( }; // create right/left bound - LineString2d right_bound; - LineString2d left_bound; + LineString2d lanelet_right_bound; + LineString2d lanelet_left_bound; for (const auto & lanelet : route_lanelets) { for (const auto & point : lanelet.rightBound()) { - boost::geometry::append(right_bound, Point2d(point.x(), point.y())); + boost::geometry::append(lanelet_right_bound, Point2d(point.x(), point.y())); } for (const auto & point : lanelet.leftBound()) { - boost::geometry::append(left_bound, Point2d(point.x(), point.y())); + boost::geometry::append(lanelet_left_bound, Point2d(point.x(), point.y())); } } + // calculate curvature + SplineInterpolationPoints2d centerline_spline(centerline); + const auto curvature_vec = centerline_spline.getSplineInterpolatedCurvatures(); + const double curvature_threshold = vehicle_info_.calcCurvatureFromSteerAngle( + vehicle_info_.max_steer_angle_rad - max_steer_angle_margin); + // calculate the distance between footprint and right/left bounds MarkerArray marker_array; double min_dist = std::numeric_limits::max(); - for (size_t i = 0; i < optimized_traj_points.size(); ++i) { - const auto & traj_point = optimized_traj_points.at(i); + double max_curvature = std::numeric_limits::min(); + for (size_t i = 0; i < centerline.size(); ++i) { + const auto & traj_point = centerline.at(i); const auto footprint_poly = create_vehicle_footprint(traj_point.pose, vehicle_info_); - const double dist_to_right = boost::geometry::distance(footprint_poly, right_bound); - const double dist_to_left = boost::geometry::distance(footprint_poly, left_bound); + const double dist_to_right = boost::geometry::distance(footprint_poly, lanelet_right_bound); + const double dist_to_left = boost::geometry::distance(footprint_poly, lanelet_left_bound); const double min_dist_to_bound = std::min(dist_to_right, dist_to_left); if (min_dist_to_bound < min_dist) { @@ -622,46 +779,111 @@ void StaticCenterlineGeneratorNode::evaluate( // create marker const auto marker_color_opt = get_marker_color(min_dist_to_bound); + const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); if (marker_color_opt) { const auto & marker_color = marker_color_opt.get(); // add footprint marker - const auto footprint_marker = - utils::create_footprint_marker(footprint_poly, marker_color, now(), i); - autoware::universe_utils::appendMarkerArray(footprint_marker, &marker_array); + const auto footprint_marker = utils::create_footprint_marker( + footprint_poly, 0.05, marker_color.at(0), marker_color.at(1), marker_color.at(2), 0.7, + now(), i); + marker_array.markers.push_back(footprint_marker); // add text of distance to bounds marker - const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); - const auto text_marker = - utils::create_distance_text_marker(text_pose, min_dist_to_bound, marker_color, now(), i); - autoware::universe_utils::appendMarkerArray(text_marker, &marker_array); + const auto text_marker = utils::create_text_marker( + "unsafe_footprints_distance", text_pose, min_dist_to_bound, marker_color.at(0), + marker_color.at(1), marker_color.at(2), 0.999, now(), i); + marker_array.markers.push_back(text_marker); + } + + const double curvature = curvature_vec.at(i); + const auto text_marker = + utils::create_text_marker("curvature", text_pose, curvature, 0.05, 0.05, 0.0, 0.9, now(), i); + marker_array.markers.push_back(text_marker); + + if (max_curvature < std::abs(curvature)) { + max_curvature = std::abs(curvature); } } - pub_debug_unsafe_footprints_->publish(marker_array); + // publish left boundary + const auto left_bound = convertToGeometryPoints(lanelet_left_bound); + const auto right_bound = convertToGeometryPoints(lanelet_right_bound); + + marker_array.markers.push_back( + utils::create_points_marker("left_bound", left_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); + marker_array.markers.push_back( + utils::create_points_marker("right_bound", right_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); + pub_debug_markers_->publish(marker_array); + + // show the validation results + // 1. distance from footprints to road boundaries + const bool are_footprints_inside_lanelets = [&]() { + std::cerr << "1. Footprints inside Lanelets:" << std::endl; + if (dist_thresh_to_road_border < min_dist) { + std::cerr << " The generated centerline is inside the lanelet. (threshold:" + << dist_thresh_to_road_border << " < actual:" << min_dist << ")" << std::endl + << " Passed." << std::endl; + return true; + } + std::cerr << RED_TEXT + << " The generated centerline is outside the lanelet. (actual:" << min_dist + << " <= threshold:" << dist_thresh_to_road_border << ")" << std::endl + << " Failed." << RESET_TEXT << std::endl; + return false; + }(); + // 2. centerline's curvature + const bool is_curvature_low = [&]() { + std::cerr << "2. Curvature:" << std::endl; + if (max_curvature < curvature_threshold) { + std::cerr << " The generated centerline has no high curvature. (actual:" << max_curvature + << " < threshold:" << curvature_threshold << ")" + << " Passed." << std::endl; + return true; + } + std::cerr << RED_TEXT << " The generated centerline has a too high curvature. (threshold:" + << curvature_threshold << " <= actual:" << max_curvature << ")" + << " Failed." << RESET_TEXT << std::endl; + return false; + }(); + // 3. result + std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl; + if (are_footprints_inside_lanelets && is_curvature_low) { + std::cerr << BOLD_TEXT << " Passed!" << RESET_TEXT << std::endl; + } else { + std::cerr << BOLD_TEXT << RED_TEXT << " Failed!" << RESET_TEXT << std::endl; + } - RCLCPP_INFO(get_logger(), "Minimum distance to road is %f [m]", min_dist); + std::cerr << "###################################################################################" + "#############################" + << std::endl + << std::endl; + RCLCPP_INFO(get_logger(), "Validated the generated centerline."); } -void StaticCenterlineGeneratorNode::save_map( - const std::string & lanelet2_output_file_path, const CenterlineWithRoute & centerline_with_route) +void StaticCenterlineGeneratorNode::save_map() { if (!route_handler_ptr_) { return; } - const auto & c = centerline_with_route; - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids); + const auto & centerline = centerline_handler_.get_selected_centerline(); + const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); + + const auto lanelet2_output_file_path = getRosParameter("lanelet2_output_file_path"); + + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // update centerline in map - utils::update_centerline(original_map_ptr_, route_lanelets, c.centerline); + utils::update_centerline(original_map_ptr_, route_lanelets, centerline); RCLCPP_INFO(get_logger(), "Updated centerline in map."); // save map with modified center line - std::filesystem::create_directory("/tmp/static_centerline_generator"); // TODO(murooka) + std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); const auto map_projector = geography_utils::get_lanelet2_projector(*map_projector_info_); lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); - RCLCPP_INFO(get_logger(), "Saved map."); + RCLCPP_INFO( + get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); // copy the output LL2 map to the temporary file for debugging const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"}; diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp index 2f25a064dca2a..42033f7956bfd 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -15,6 +15,7 @@ #ifndef STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #define STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +#include "autoware/universe_utils/ros/parameter.hpp" #include "autoware_static_centerline_generator/srv/load_map.hpp" #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" @@ -23,7 +24,7 @@ #include "rclcpp/rclcpp.hpp" #include "type_alias.hpp" -#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/empty.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" #include "tier4_map_msgs/msg/map_projector_info.hpp" @@ -45,12 +46,67 @@ struct CenterlineWithRoute std::vector centerline{}; std::vector route_lane_ids{}; }; +struct CenterlineHandler +{ + CenterlineHandler() = default; + explicit CenterlineHandler(const CenterlineWithRoute & centerline_with_route) + : whole_centerline_with_route(centerline_with_route), + start_index(0), + end_index(centerline_with_route.centerline.size() - 1) + { + } + std::vector get_selected_centerline() const + { + if (!whole_centerline_with_route) { + return std::vector{}; + } + const auto & centerline_begin = whole_centerline_with_route->centerline.begin(); + return std::vector( + centerline_begin + start_index, centerline_begin + end_index + 1); + } + std::vector get_route_lane_ids() const + { + if (!whole_centerline_with_route) { + return std::vector{}; + } + return whole_centerline_with_route->route_lane_ids; + } + bool is_valid() const { return whole_centerline_with_route && start_index < end_index; } + bool update_start_index(const int arg_start_index) + { + if (whole_centerline_with_route && arg_start_index < end_index) { + start_index = arg_start_index; + return true; + } + return false; + } + bool update_end_index(const int arg_end_index) + { + if (whole_centerline_with_route && start_index < arg_end_index) { + end_index = arg_end_index; + return true; + } + return false; + } + + std::optional whole_centerline_with_route{std::nullopt}; + int start_index{}; + int end_index{}; +}; + +struct RoadBounds +{ + std::vector left_bound; + std::vector right_bound; +}; class StaticCenterlineGeneratorNode : public rclcpp::Node { public: explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); - void run(); + void generate_centerline(); + void validate(); + void save_map(); private: // load map @@ -59,33 +115,41 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); // plan route - std::vector plan_route( + std::vector plan_route_by_lane_ids( const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); + std::vector plan_route( + const geometry_msgs::msg::Pose & start_center_pose, + const geometry_msgs::msg::Pose & end_center_pose); + void on_plan_route( const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); // plan centerline - CenterlineWithRoute generate_centerline_with_route(); + CenterlineWithRoute generate_whole_centerline_with_route(); std::vector get_route_lane_ids_from_points( const std::vector & points); void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); - void update_centerline_range(const int traj_start_index, const int traj_end_index); - void evaluate( - const std::vector & route_lane_ids, - const std::vector & optimized_traj_points); - void save_map( - const std::string & lanelet2_output_file_path, - const CenterlineWithRoute & centerline_with_route); + void visualize_selected_centerline(); + RoadBounds update_road_boundary(const std::vector & centerline); + void evaluate(); + + // parameter + template + T getRosParameter(const std::string & param_name) + { + return autoware::universe_utils::getOrDeclareParameter(*this, param_name); + } lanelet::LaneletMapPtr original_map_ptr_{nullptr}; LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; std::unique_ptr map_projector_info_{nullptr}; - std::pair traj_range_indices_{0, 0}; - std::optional centerline_with_route_{std::nullopt}; + CenterlineHandler centerline_handler_; + + float footprint_margin_for_road_bound_{0.0}; enum class CenterlineSource { OptimizationTrajectoryBase = 0, @@ -96,15 +160,19 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; + rclcpp::Publisher::SharedPtr pub_validation_results_{nullptr}; + rclcpp::Publisher::SharedPtr pub_debug_ego_footprint_bounds_{nullptr}; + rclcpp::Publisher::SharedPtr pub_debug_markers_{nullptr}; // subscriber rclcpp::Subscription::SharedPtr sub_traj_start_index_; rclcpp::Subscription::SharedPtr sub_traj_end_index_; - rclcpp::Subscription::SharedPtr sub_save_map_; + rclcpp::Subscription::SharedPtr sub_save_map_; + rclcpp::Subscription::SharedPtr sub_validate_; rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; + rclcpp::Subscription::SharedPtr sub_footprint_margin_for_road_bound_; // service rclcpp::Service::SharedPtr srv_load_map_; diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index c5157acc2b525..2b7b99bfe81be 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -40,6 +40,7 @@ using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathWithLaneId; +using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index de362e3af5d7f..1a8e0eae2fbd9 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -23,6 +23,7 @@ #include #include +#include namespace autoware::static_centerline_generator { @@ -151,6 +152,7 @@ void update_centerline( auto & lanelet_ref = lanelets_ref.at(lanelet_idx); const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); + // TODO(murooka) This does not work with L-crank map. const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); if (is_inside) { const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); @@ -181,20 +183,17 @@ void update_centerline( } } -MarkerArray create_footprint_marker( - const LinearRing2d & footprint_poly, const std::array & marker_color, - const rclcpp::Time & now, const size_t idx) +Marker create_footprint_marker( + const LinearRing2d & footprint_poly, const double width, const double r, const double g, + const double b, const double a, const rclcpp::Time & now, const size_t idx) { - const double r = marker_color.at(0); - const double g = marker_color.at(1); - const double b = marker_color.at(2); - auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints", idx, visualization_msgs::msg::Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, a)); marker.header.stamp = now; + // TODO(murooka) Ideally, the following is unnecessary for the topic of transient local. marker.lifetime = rclcpp::Duration(0, 0); for (const auto & point : footprint_poly) { @@ -206,38 +205,79 @@ MarkerArray create_footprint_marker( marker.points.push_back(geom_point); } marker.points.push_back(marker.points.front()); - - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.push_back(marker); - - return marker_array; + return marker; } -MarkerArray create_distance_text_marker( - const geometry_msgs::msg::Pose & pose, const double dist, - const std::array & marker_color, const rclcpp::Time & now, const size_t idx) +Marker create_text_marker( + const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, + const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx) { - const double r = marker_color.at(0); - const double g = marker_color.at(1); - const double b = marker_color.at(2); - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + "map", rclcpp::Clock().now(), ns, idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerColor(r, g, b, a)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); std::stringstream ss; - ss << std::setprecision(2) << dist; + ss << std::setprecision(2) << value; marker.text = ss.str(); - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.push_back(marker); + return marker; +} + +Marker create_points_marker( + const std::string & ns, const std::vector & points, const double width, + const double r, const double g, const double b, const double a, const rclcpp::Time & now) +{ + auto marker = autoware::universe_utils::createDefaultMarker( + "map", now, ns, 1, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, a)); + marker.lifetime = rclcpp::Duration(0, 0); + marker.points = points; + return marker; +} + +MarkerArray create_delete_all_marker_array( + const std::vector & ns_vec, const rclcpp::Time & now) +{ + Marker marker; + marker.header.stamp = now; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + + MarkerArray marker_array; + for (const auto & ns : ns_vec) { + marker.ns = ns; + marker_array.markers.push_back(marker); + } return marker_array; } + +std::pair, std::vector> +calcBoundsFromLanelets(const lanelet::ConstLanelets lanelets) +{ + std::vector left_bound; + std::vector right_bound; + for (const auto & lanelet : lanelets) { + for (const auto & lanelet_left_bound_point : lanelet.leftBound()) { + geometry_msgs::msg::Point left_bound_point; + left_bound_point.x = lanelet_left_bound_point.x(); + left_bound_point.y = lanelet_left_bound_point.y(); + left_bound_point.z = lanelet_left_bound_point.z(); + left_bound.push_back(left_bound_point); + } + for (const auto & lanelet_right_bound_point : lanelet.rightBound()) { + geometry_msgs::msg::Point right_bound_point; + right_bound_point.x = lanelet_right_bound_point.x(); + right_bound_point.y = lanelet_right_bound_point.y(); + right_bound_point.z = lanelet_right_bound_point.z(); + right_bound.push_back(right_bound_point); + } + } + return std::make_pair(left_bound, right_bound); +} } // namespace utils } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp index 70a7e61e19b18..e7fd51b30e658 100644 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ b/planning/autoware_static_centerline_generator/src/utils.hpp @@ -22,6 +22,7 @@ #include #include +#include #include namespace autoware::static_centerline_generator @@ -45,13 +46,23 @@ void update_centerline( lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, const std::vector & new_centerline); -MarkerArray create_footprint_marker( - const LinearRing2d & footprint_poly, const std::array & marker_color, - const rclcpp::Time & now, const size_t idx); +Marker create_footprint_marker( + const LinearRing2d & footprint_poly, const double width, const double r, const double g, + const double b, const double a, const rclcpp::Time & now, const size_t idx); -MarkerArray create_distance_text_marker( - const geometry_msgs::msg::Pose & pose, const double dist, - const std::array & marker_color, const rclcpp::Time & now, const size_t idx); +Marker create_text_marker( + const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, + const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx); + +Marker create_points_marker( + const std::string & ns, const std::vector & points, const double width, + const double r, const double g, const double b, const double a, const rclcpp::Time & now); + +MarkerArray create_delete_all_marker_array( + const std::vector & ns_vec, const rclcpp::Time & now); + +std::pair, std::vector> +calcBoundsFromLanelets(const lanelet::ConstLanelets lanelets); } // namespace utils } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 new file mode 100644 index 0000000000000..ed3448772b794 Binary files /dev/null and b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 differ diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 similarity index 100% rename from planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3 rename to planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 07f8f3e30e659..4bb0630d13942 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -38,7 +38,7 @@ def generate_test_description(): output="screen", parameters=[ {"lanelet2_map_path": lanelet2_map_path}, - {"run_background": False}, + {"mode": "AUTO"}, {"rviz": False}, {"centerline_source": "optimization_trajectory_base"}, {"lanelet2_input_file_path": lanelet2_map_path}, diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index aefbd9ef20f84..5c66f5b906bba 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -1013,7 +1013,6 @@ VelocitySmootherNode::AlgorithmType VelocitySmootherNode::getAlgorithmType( } throw std::domain_error("[VelocitySmootherNode] undesired algorithm is selected."); - return AlgorithmType::INVALID; } double VelocitySmootherNode::calcTravelDistance() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index 8b411340e4ddc..2bd52850d0a3f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -1471,7 +1471,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( return prev_object->lat_offset_to_avoid->min_value; }(); const double filtered_min_bound_lat_offset = - (prev_min_lat_avoid_to_offset.has_value() & enable_lowpass_filter) + (prev_min_lat_avoid_to_offset.has_value() && enable_lowpass_filter) ? signal_processing::lowpassFilter( min_bound_lat_offset, *prev_min_lat_avoid_to_offset, parameters_->lpf_gain_for_lat_avoid_to_offset) @@ -1556,7 +1556,7 @@ DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( return prev_object->lat_offset_to_avoid->min_value; }(); const double filtered_min_bound_pos = - (prev_min_lat_avoid_to_offset.has_value() & enable_lowpass_filter) + (prev_min_lat_avoid_to_offset.has_value() && enable_lowpass_filter) ? signal_processing::lowpassFilter( bound_pos.min_value, *prev_min_lat_avoid_to_offset, parameters_->lpf_gain_for_lat_avoid_to_offset) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 1be3e04914cda..3e155aba0af2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -382,13 +382,14 @@ In addition, the safety check has a time hysteresis, and if the path is judged " ### Parameters for safety check -| Name | Unit | Type | Description | Default value | -| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | -| enable_safety_check | [-] | bool | flag whether to use safety check | true | -| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | -| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | -| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | -| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| Name | Unit | Type | Description | Default value | +| :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | +| enable_safety_check | [-] | bool | flag whether to use safety check | true | +| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | +| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| `collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### Parameters for RSS safety check diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 915d256aba1db..53e06631a81d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -180,6 +180,7 @@ time_horizon: 10.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 3.1416 # temporary backward_path_length: 100.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 2ac0b9493e67a..6caeb411f6680 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2319,8 +2319,10 @@ std::pair GoalPlannerModule::isSafePath( return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_over_path, ego_predicted_path, filtered_objects, collision_check, planner_data->parameters, safety_check_params->rss_params, - objects_filtering_params->use_all_predicted_path, hysteresis_factor); - } else if (parameters.safety_check_params.method == "integral_predicted_polygon") { + objects_filtering_params->use_all_predicted_path, hysteresis_factor, + safety_check_params->collision_check_yaw_diff_threshold); + } + if (parameters.safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( ego_predicted_path, vehicle_info_, filtered_objects, objects_filtering_params->check_all_predicted_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 901fe351fc68b..b079db9babf31 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -357,6 +357,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); p.safety_check_params.hysteresis_factor_expand_rate = node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = @@ -778,6 +780,9 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "hysteresis_factor_expand_rate", p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); updateParam( parameters, safety_check_ns + "backward_path_length", p->safety_check_params.backward_path_length); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index a0870fe428c8a..5790be377b7aa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -725,6 +725,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | | `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | | `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| `safety_check.collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### safety constraints during lane change path is computed diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 1ab33514c5f24..d2f695071649a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -30,6 +30,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index e2afb6abeec03..ffd2754acc38f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -137,6 +137,7 @@ struct LaneChangeParameters // safety check bool allow_loose_check_for_cancel{true}; + double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 61ff54e8f99f3..22c93f7d4bfd5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // safety check p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.collision_check_yaw_diff_threshold = getOrDeclareParameter( + *node, parameter("safety_check.collision_check_yaw_diff_threshold")); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 505bb3ef93340..fd96701fe088e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -2048,6 +2049,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( lane_change_parameters_->lane_expansion_left_offset, lane_change_parameters_->lane_expansion_right_offset); + constexpr double collision_check_yaw_diff_threshold{M_PI}; + for (const auto & obj : collision_check_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( @@ -2056,7 +2059,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - get_max_velocity_for_safety_check(), current_debug_data.second); + get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md index 4733fe7832da6..5511202c390b1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md @@ -48,7 +48,7 @@ $$ rss_{dist} = v_{rear} (t_{reaction} + t_{margin}) + \frac{v_{rear}^2}{2|a_{rear, decel}|} - \frac{v_{front}^2}{2|a_{front, decel|}} $$ -where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. +where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. Note that RSS distance is normally used for objects traveling in the same direction, if the yaw difference between a given ego pose and object pose is more than a user-defined yaw difference threshold, the rss collision check will be skipped for that specific pair of poses. #### 5. Create extended ego and target object polygons @@ -56,7 +56,7 @@ In this step, we compute extended ego and target object polygons. The extended p ![extended_polygons](../images/path_safety_checker/extended_polygons.drawio.svg) -As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin +As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin. #### 6. Check overlap diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 313f9df471938..0e7d1cee65f02 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -23,6 +23,7 @@ #include +#include #include #include #include @@ -203,7 +204,9 @@ struct SafetyCheckParams /// possible values: ["RSS", "integral_predicted_polygon"] double keep_unsafe_time{0.0}; ///< Time to keep unsafe before changing to safe. double hysteresis_factor_expand_rate{ - 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + double collision_check_yaw_diff_threshold{ + 3.1416}; ///< threshold yaw difference between ego and object. double backward_path_length{0.0}; ///< Length of the backward lane for path generation. double forward_path_length{0.0}; ///< Length of the forward path lane for path generation. RSSparams rss_params{}; ///< Parameters related to the RSS model. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 3f1094dfd70b6..d0d19b6b8fed2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -98,7 +99,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor); + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th); /** * @brief Iterate the points in the ego and target's predicted path and @@ -111,6 +113,8 @@ bool checkSafetyWithRSS( * @param common_parameters The common parameters used in behavior path planner. * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param yaw_difference_th maximum yaw difference between any given ego path pose and object + * predicted path pose. * @param debug The debug information for collision checking. * @return true if distance is safe. */ @@ -120,7 +124,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug); + const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug); /** * @brief Iterate the points in the ego and target's predicted path and @@ -142,7 +146,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug); + const double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th, + CollisionCheckDebug & debug); bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 7bb8c13aa65fb..b05d869cca6b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -26,6 +26,9 @@ #include #include +#include + +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -482,7 +485,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor) + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th) { // Check for collisions with each predicted path of the object const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { @@ -495,7 +499,7 @@ bool checkSafetyWithRSS( obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { const bool has_collision = !utils::path_safety_checker::checkCollision( planned_path, ego_predicted_path, object, obj_path, parameters, rss_params, - hysteresis_factor, current_debug_data.second); + hysteresis_factor, yaw_difference_th, current_debug_data.second); utils::path_safety_checker::updateCollisionCheckDebugMap( debug_map, current_debug_data, !has_collision); @@ -559,11 +563,12 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug) + const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug) { const auto collided_polygons = getCollidedPolygons( planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, - rss_parameters, hysteresis_factor, std::numeric_limits::max(), debug); + rss_parameters, hysteresis_factor, std::numeric_limits::max(), yaw_difference_th, + debug); return collided_polygons.empty(); } @@ -573,7 +578,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug) + double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th, + CollisionCheckDebug & debug) { { debug.ego_predicted_path = predicted_ego_path; @@ -604,6 +610,11 @@ std::vector getCollidedPolygons( const auto & ego_polygon = interpolated_data->poly; const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit); + const double ego_yaw = tf2::getYaw(ego_pose.orientation); + const double object_yaw = tf2::getYaw(obj_pose.orientation); + const double yaw_difference = autoware::universe_utils::normalizeRadian(ego_yaw - object_yaw); + if (std::abs(yaw_difference) > yaw_difference_th) continue; + // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 46ca155d04daa..032e5a42069b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -471,14 +471,15 @@ std::vector interpolatePose( { std::vector interpolated_poses{}; // output - const std::vector base_s{ - 0, autoware::universe_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const double distance = + autoware::universe_utils::calcDistance2d(start_pose.position, end_pose.position); + const std::vector base_s{0.0, distance}; const std::vector base_x{start_pose.position.x, end_pose.position.x}; const std::vector base_y{start_pose.position.y, end_pose.position.y}; std::vector new_s; constexpr double eps = 0.3; // prevent overlapping - for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + for (double s = eps; s < distance - eps; s += resample_interval) { new_s.push_back(s); } @@ -491,7 +492,7 @@ std::vector interpolatePose( for (size_t i = 0; i < interpolated_x.size(); ++i) { Pose pose{}; pose = autoware::universe_utils::calcInterpolatedPose( - end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + end_pose, start_pose, (distance - new_s.at(i)) / distance); pose.position.x = interpolated_x.at(i); pose.position.y = interpolated_y.at(i); pose.position.z = end_pose.position.z; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 55a6c6ff39d30..4ab1ef0d1fc23 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -734,12 +734,11 @@ double getDistanceToCrosswalk( boost::geometry::intersection(centerline, polygon, points_intersection); for (const auto & point : points_intersection) { - lanelet::ConstLanelets lanelets = {llt}; Pose pose_point; pose_point.position.x = point.x(); pose_point.position.y = point.y(); const lanelet::ArcCoordinates & arc_crosswalk = - lanelet::utils::getArcCoordinates(lanelets, pose_point); + lanelet::utils::getArcCoordinates({llt}, pose_point); const double distance_to_crosswalk = arc_crosswalk.length; if (distance_to_crosswalk < min_distance_to_crosswalk) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md index 066708891652a..dba4a0cfce5e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md @@ -436,17 +436,18 @@ Parameters under `target_filtering` are related to filtering target objects for Parameters under `safety_check_params` define the configuration for safety check. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | -| enable_safety_check | - | bool | Flag to enable safety check | true | -| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | -| publish_debug_marker | - | bool | Flag to publish debug markers | false | -| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | -| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | -| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | -| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | -| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | -| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------- | :------------ | +| enable_safety_check | - | bool | Flag to enable safety check | true | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | +| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| collision_check_yaw_diff_threshold | - | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 1.578 | ## **Path Generation** diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index ce7ead87b54c7..d39a320a19e14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -143,8 +143,10 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 1.578 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 7554098414442..b07d6497463ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -77,7 +77,7 @@ struct PullOutStatus // record if the ego has departed from the start point bool has_departed{false}; - PullOutStatus() {} + PullOutStatus() = default; }; class StartPlannerModule : public SceneModuleInterface @@ -90,7 +90,7 @@ class StartPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); - ~StartPlannerModule() + ~StartPlannerModule() override { if (freespace_planner_timer_) { freespace_planner_timer_->cancel(); @@ -296,16 +296,16 @@ class StartPlannerModule : public SceneModuleInterface PathWithLaneId getCurrentPath() const; void planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority); + const Pose & goal_pose, const std::string & search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, - const double velocity_threshold, const double object_check_backward_distance, - const double object_check_forward_distance) const; + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, + const double velocity_threshold, const double object_check_forward_distance, + const double object_check_backward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasReachedFreespaceEnd() const; bool hasReachedPullOutEnd() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 9451bb3870648..873bab1043485 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -283,6 +283,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(safety_check_ns + "forward_path_length"); p.safety_check_params.publish_debug_marker = node->declare_parameter(safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); } // RSSparams @@ -298,6 +300,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); p.safety_check_params.rss_params.longitudinal_velocity_delta_time = node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + node->declare_parameter(rss_ns + "extended_polygon_policy"); } // surround moving obstacle check @@ -367,7 +371,6 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "extra_width_margin_for_rear_obstacle", p->extra_width_margin_for_rear_obstacle); - updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( @@ -658,6 +661,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "publish_debug_marker", p->safety_check_params.publish_debug_marker); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); } const std::string rss_ns = safety_check_ns + "rss_params."; { @@ -676,6 +682,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, rss_ns + "longitudinal_velocity_delta_time", p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + updateParam( + parameters, rss_ns + "extended_polygon_policy", + p->safety_check_params.rss_params.extended_polygon_policy); } std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9400f3ff40286..08d5ea4365575 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -34,7 +34,9 @@ #include #include +#include #include +#include #include #include #include @@ -65,7 +67,7 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); - autoware::lane_departure_checker::Param lane_departure_checker_params; + autoware::lane_departure_checker::Param lane_departure_checker_params{}; lane_departure_checker_params.footprint_extra_margin = parameters->lane_departure_check_expansion_margin; @@ -104,7 +106,7 @@ void StartPlannerModule::onFreespacePlannerTimer() std::optional current_status_opt{std::nullopt}; std::optional parameters_opt{std::nullopt}; std::optional pull_out_status_opt{std::nullopt}; - bool is_stopped; + bool is_stopped{false}; // making a local copy of thread sensitive data { @@ -501,7 +503,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // Get the closest target obj width in the relevant lanes const auto closest_object_width = std::invoke([&]() -> std::optional { double arc_length_to_closet_object = std::numeric_limits::max(); - double closest_object_width = -1.0; + std::optional closest_object_width = std::nullopt; std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { @@ -512,7 +514,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const arc_length_to_closet_object = arc_length; closest_object_width = o.shape.dimensions.y; }); - if (closest_object_width < 0.0) return std::nullopt; return closest_object_width; }); if (!closest_object_width) return false; @@ -837,7 +838,7 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority) + const Pose & goal_pose, const std::string & search_priority) { if (start_pose_candidates.empty()) return; @@ -1112,8 +1113,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() waitApproval(); // To enable approval of the forward path, the RTC status is removed. removeRTCStatus(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); + for (auto & itr : uuid_map_) { + itr.second = generateUUID(); } } @@ -1303,7 +1304,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; @@ -1422,10 +1423,12 @@ bool StartPlannerModule::isSafePath() const merged_target_object.insert( merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), target_objects_on_lane.on_shoulder_lane.end()); + return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, - objects_filtering_params_->use_all_predicted_path, hysteresis_factor); + objects_filtering_params_->use_all_predicted_path, hysteresis_factor, + safety_check_params_->collision_check_yaw_diff_threshold); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1693,7 +1696,7 @@ void StartPlannerModule::setDebugData() // safety check if (parameters_->safety_check_params.enable_safety_check) { - if (debug_data_.ego_predicted_path.size() > 0) { + if (!debug_data_.ego_predicted_path.empty()) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add( @@ -1702,7 +1705,7 @@ void StartPlannerModule::setDebugData() debug_marker_); } - if (debug_data_.filtered_objects.objects.size() > 0) { + if (!debug_data_.filtered_objects.objects.empty()) { add( createObjectsMarkerArray( debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9), diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index 33abc8eaf40ed..ec139b9a8b099 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -485,11 +485,6 @@ title Filtering flow for vehicle type objects start partition isSatisfiedWithVehicleCodition() { -if(object is force avoidance target ?) then (yes) -#FF006C :return true; -stop -else (\n no) - if(isNeverAvoidanceTarget()) then (yes) #00FFB1 :return false; stop @@ -537,7 +532,6 @@ endif endif endif endif -endif #00FFB1 :return false; stop } @@ -766,7 +760,7 @@ Basically, this module tries to generate avoidance path in order to keep lateral But if there isn't enough space to keep `soft_margin` distance, this module shortens soft constraint lateral margin. The parameter `soft_margin` is a maximum value of soft constraint, and actual soft margin can be a value between 0.0 and `soft_margin`. On the other hand, this module definitely keeps `hard_margin` or `hard_margin_for_parked_vehicle` depending on the situation. Thus, the minimum value of total lateral margin is `hard_margin`/`hard_margin_for_parked_vehicle`, and the maximum value is the sum of `hard_margin`/`hard_margin_for_parked_vehicle` and `soft_margin`. -Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_opposite_lane` to `false`. +Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_lane_type` to `same_direction_lane`. ![fig](./images/path_generation/adjust_margin.png) @@ -790,7 +784,7 @@ On the other hand, if the lateral distance is larger than `hard_margin`/`hard_ma ### When there is not enough space -This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_opposite_lane` to `false`. +This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_lane_type` to `same_direction_lane`. ![fig](./images/path_generation/do_nothing.png) @@ -810,15 +804,19 @@ Usable lane for avoidance module can be selected by config file. ```yaml ... - use_adjacent_lane: true - use_opposite_lane: true + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" ``` -When user set parameter both `use_adjacent_lane` and `use_opposite_lane` to `true`, it is possible to use opposite lane. +When user set parameter `use_lane_type` to `opposite_direction_lane`, it is possible to use opposite lane. ![fig](./images/path_generation/opposite_direction.png) -When user only set parameter `use_adjacent_lane` to `true`, the module doesn't create path that overlaps opposite lane. +When user set parameter `use_lane_type` to `same_direction_lane`, the module doesn't create path that overlaps opposite lane. ![fig](./images/path_generation/same_direction.png) @@ -978,21 +976,25 @@ This module supports drivable area expansion for following polygons defined in H Please set the flags to `true` when user wants to make it possible to use those areas in avoidance maneuver. ```yaml +# drivable lane setting. this module is able to use not only current lane but also right/left lane +# if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap. +# "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. +# "same_direction_lane" : this module uses same direction lane to avoid object if need. +# "opposite_direction_lane": this module uses both same direction and opposite direction lane. +use_lane_type: "opposite_direction_lane" # drivable area setting -use_adjacent_lane: true -use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true use_freespace_areas: true ``` -| | | | -| --------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| adjacent lane | ![fig](./images/advanced/avoidance_same_direction.png) | | -| opposite lane | ![fig](./images/advanced/avoidance_opposite_direction.png) | | -| intersection area | ![fig](./images/advanced/avoidance_intersection.png) | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) | -| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png) | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) | -| freespace area | ![fig](./images/advanced/avoidance_freespace.png) | The freespace area is defined on Lanelet map. (unstable) | +| | | | +| -------------------------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| use_lane_type: same_direction_lane | ![fig](./images/advanced/avoidance_same_direction.png) | | +| use_lane_type: opposite_direction_lane | ![fig](./images/advanced/avoidance_opposite_direction.png) | | +| intersection area | ![fig](./images/advanced/avoidance_intersection.png) | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) | +| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png) | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) | +| freespace area | ![fig](./images/advanced/avoidance_freespace.png) | The freespace area is defined on Lanelet map. (unstable) | ## Future extensions / Unimplemented parts diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 324be9a73439e..3087ccc93934b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -5,9 +5,13 @@ resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" # drivable area setting - use_adjacent_lane: true - use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true use_freespace_areas: true @@ -174,6 +178,7 @@ safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 1.5 # [-] hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index f05230f4ec19d..d02b39047e71c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -101,12 +101,8 @@ struct AvoidanceParameters // computational cost for latter modules. double resample_interval_for_output = 3.0; - // enable avoidance to be perform only in lane with same direction - bool use_adjacent_lane{true}; - - // enable avoidance to be perform in opposite lane direction - // to use this, enable_avoidance_over_same_direction need to be set to true. - bool use_opposite_lane{true}; + // drivable lane config + std::string use_lane_type{"current_lane"}; // if this param is true, it reverts avoidance path when the path is no longer needed. bool enable_cancel_maneuver{false}; @@ -223,6 +219,8 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; + double collision_check_yaw_diff_threshold{3.1416}; + bool consider_front_overhang{true}; bool consider_rear_overhang{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index b761939589968..84cf7c4e33d26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -46,8 +46,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // drivable area { const std::string ns = "avoidance."; - p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); - p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_lane_type = getOrDeclareParameter(*node, ns + "use_lane_type"); p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); @@ -199,6 +198,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); p.hysteresis_factor_safe_count = getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + p.collision_check_yaw_diff_threshold = + getOrDeclareParameter(*node, ns + "collision_check_yaw_diff_threshold"); } // safety check predicted path params diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 3e6f0ef1f61a3..db3215fa8d238 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -17,19 +17,16 @@ "default": "4.0" }, "path_generation_method": { + "type": "string", "enum": ["shift_line_base", "optimization_base", "both"], "description": "Path generation method.", "default": "shift_line_base" }, - "use_adjacent_lane": { - "type": "boolean", - "description": "Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.", - "default": "true" - }, - "use_opposite_lane": { - "type": "boolean", - "description": "Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects.", - "default": "true" + "use_lane_type": { + "type": "string", + "enum": ["current_lane", "same_direction_lane", "opposite_direction_lane"], + "description": "Drivable lane configuration.", + "default": "opposite_direction_lane" }, "use_hatched_road_markings": { "type": "boolean", @@ -926,6 +923,11 @@ "description": "Hysteresis count that be used for chattering prevention.", "default": 10 }, + "collision_check_yaw_diff_threshold": { + "type": "number", + "description": "Max yaw difference between ego and object when doing collision check", + "default": 3.1416 + }, "min_velocity": { "type": "number", "description": "Minimum velocity of the ego vehicle's predicted path.", @@ -1441,8 +1443,7 @@ "resample_interval_for_planning", "resample_interval_for_output", "path_generation_method", - "use_adjacent_lane", - "use_opposite_lane", + "use_lane_type", "use_hatched_road_markings", "use_intersection_areas", "use_freespace_areas", diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b0fd1b5c1b8ab..824466a1ad3ad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -781,7 +781,8 @@ bool StaticObstacleAvoidanceModule::isSafePath( for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - hysteresis_factor, current_debug_data.second)) { + hysteresis_factor, parameters_->collision_check_yaw_diff_threshold, + current_debug_data.second)) { utils::path_safety_checker::updateCollisionCheckDebugMap( debug.collision_check, current_debug_data, false); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 70de9e9499917..8d0f9904b5bad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -2238,14 +2238,16 @@ DrivableLanes generateExpandedDrivableLanes( current_drivable_lanes.left_lane = lanelet; current_drivable_lanes.right_lane = lanelet; - if (!parameters->use_adjacent_lane) { + if (parameters->use_lane_type == "current_lane") { return current_drivable_lanes; } + const auto use_opposite_lane = parameters->use_lane_type == "opposite_direction_lane"; + // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( - target_lane, parameters->use_opposite_lane, true); + const auto all_left_lanelets = + route_handler->getAllLeftSharedLinestringLanelets(target_lane, use_opposite_lane, true); if (!all_left_lanelets.empty()) { current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet pushUniqueVector( @@ -2254,8 +2256,8 @@ DrivableLanes generateExpandedDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( - target_lane, parameters->use_opposite_lane, true); + const auto all_right_lanelets = + route_handler->getAllRightSharedLinestringLanelets(target_lane, use_opposite_lane, true); if (!all_right_lanelets.empty()) { current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet pushUniqueVector( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 7abd88d59f398..9cbe962e88cae 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -83,7 +83,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Get stop point const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, lane_id_, planner_param_.stop_margin, + original_path, stop_line, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { return true; @@ -128,7 +128,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (planner_param_.use_dead_line) { // Use '-' for margin because it's the backward distance from stop line const auto dead_line_point = arc_lane_utils::createTargetPoint( - original_path, stop_line, lane_id_, -planner_param_.dead_line_margin, + original_path, stop_line, -planner_param_.dead_line_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (dead_line_point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index d007416dccb47..0478aea34a44f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -125,7 +125,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } const auto stop_point = arc_lane_utils::createTargetPoint( - original_path, stop_line.value(), lane_id_, planner_param_.stop_margin, + original_path, stop_line.value(), planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); if (!stop_point) { setSafe(true); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md index f688318ed051e..041e0b86e0d06 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/README.md @@ -19,7 +19,7 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Occlusion Spot](../autoware_behavior_velocity_occlusion_spot_module/README.md) - [No Stopping Area](../autoware_behavior_velocity_no_stopping_area_module/README.md) - [Run Out](../autoware_behavior_velocity_run_out_module/README.md) -- [Speed Bump](../behavior_velocity_speed_bump_module/README.md) +- [Speed Bump](../autoware_behavior_velocity_speed_bump_module/README.md) When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 24d11eb9d08d5..1d256b61fa861 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -66,7 +66,6 @@ ament_cmake_ros ament_lint_auto autoware_behavior_velocity_blind_spot_module - autoware_behavior_velocity_blind_spot_module autoware_behavior_velocity_crosswalk_module autoware_behavior_velocity_detection_area_module autoware_behavior_velocity_intersection_module @@ -74,12 +73,12 @@ autoware_behavior_velocity_no_stopping_area_module autoware_behavior_velocity_occlusion_spot_module autoware_behavior_velocity_run_out_module + autoware_behavior_velocity_speed_bump_module autoware_behavior_velocity_stop_line_module autoware_behavior_velocity_traffic_light_module autoware_behavior_velocity_virtual_traffic_light_module autoware_behavior_velocity_walkway_module autoware_lint_common - behavior_velocity_speed_bump_module rosidl_interface_packages diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 29cd7a215907f..3482d7be8ec48 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -55,12 +55,6 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* - const auto get_behavior_velocity_module_config_no_prefix = [](const std::string & module) { - const auto package_name = "behavior_velocity_" + module + "_module"; - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return package_path + "/config/" + module + ".param.yaml"; - }; - const auto get_behavior_velocity_module_config = [](const std::string & module) { const auto package_name = "autoware_behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); @@ -103,7 +97,7 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config("no_stopping_area"), get_behavior_velocity_module_config("occlusion_spot"), get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config_no_prefix("speed_bump"), + get_behavior_velocity_module_config("speed_bump"), get_behavior_velocity_module_config("stop_line"), get_behavior_velocity_module_config("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 77b7d25f9f46e..769932259e3e6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -61,20 +61,9 @@ std::optional checkCollision( template std::optional findCollisionSegment( const T & path, const geometry_msgs::msg::Point & stop_line_p1, - const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id) + const geometry_msgs::msg::Point & stop_line_p2) { for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & prev_lane_ids = path.points.at(i).lane_ids; - const auto & next_lane_ids = path.points.at(i + 1).lane_ids; - - const bool is_target_lane_in_prev_lane = - std::find(prev_lane_ids.begin(), prev_lane_ids.end(), target_lane_id) != prev_lane_ids.end(); - const bool is_target_lane_in_next_lane = - std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != next_lane_ids.end(); - if (!is_target_lane_in_prev_lane && !is_target_lane_in_next_lane) { - continue; - } - const auto & p1 = autoware::universe_utils::getPoint(path.points.at(i)); // Point before collision point const auto & p2 = @@ -92,12 +81,12 @@ std::optional findCollisionSegment( template std::optional findCollisionSegment( - const T & path, const LineString2d & stop_line, const size_t target_lane_id) + const T & path, const LineString2d & stop_line) { const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); const auto stop_line_p2 = convertToGeomPoint(stop_line.at(1)); - return findCollisionSegment(path, stop_line_p1, stop_line_p2, target_lane_id); + return findCollisionSegment(path, stop_line_p1, stop_line_p2); } template @@ -194,7 +183,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse std::optional createTargetPoint( const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const size_t lane_id, const double margin, const double vehicle_offset); + const double margin, const double vehicle_offset); } // namespace arc_lane_utils } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 3e029174b288d..da6ef7262de74 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -109,10 +109,10 @@ std::optional findOffsetSegment( std::optional createTargetPoint( const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const size_t lane_id, const double margin, const double vehicle_offset) + const double margin, const double vehicle_offset) { // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); + const auto collision_segment = findCollisionSegment(path, stop_line); if (!collision_segment) { // No collision return {}; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index ddedec80dc0f3..db9a63169e565 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -49,14 +49,11 @@ TEST(findCollisionSegment, nominal) * **/ auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); - for (auto & point : path.points) { - point.lane_ids.push_back(100); - } LineString2d stop_line; stop_line.emplace_back(Point2d(3.5, 3.0)); stop_line.emplace_back(Point2d(3.5, -3.0)); - auto segment = arc_lane_utils::findCollisionSegment(path, stop_line, 100); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); EXPECT_EQ(segment->first, static_cast(3)); EXPECT_DOUBLE_EQ(segment->second.x, 3.5); EXPECT_DOUBLE_EQ(segment->second.y, 0.0); diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/CMakeLists.txt similarity index 86% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/CMakeLists.txt rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/CMakeLists.txt index f5d3a7078be9c..5262939c02ddf 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_speed_bump_module) +project(autoware_behavior_velocity_speed_bump_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/README.md similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/README.md rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/README.md diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/config/speed_bump.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/config/speed_bump.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/config/speed_bump.param.yaml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/config/speed_bump.param.yaml diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/docs/speed_bump_design.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/docs/speed_bump_design.svg similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/docs/speed_bump_design.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/docs/speed_bump_design.svg diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/docs/speed_bump_scenarios.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/docs/speed_bump_scenarios.svg similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/docs/speed_bump_scenarios.svg rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/docs/speed_bump_scenarios.svg diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/docs/speed_bump_vel_calc.png b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/docs/speed_bump_vel_calc.png similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/docs/speed_bump_vel_calc.png rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/docs/speed_bump_vel_calc.png diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml similarity index 89% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml index 805ab2f39a7ef..6c38e194a63c2 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_speed_bump_module + autoware_behavior_velocity_speed_bump_module 0.1.0 - The behavior_velocity_speed_bump_module package + The autoware_behavior_velocity_speed_bump_module package Tomoya Kimura Shumpei Wakabayashi diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/plugins.xml similarity index 71% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/plugins.xml rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/plugins.xml index 48287a7f94b4b..7a37ec217e9a5 100644 --- a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/plugins.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/debug.cpp similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/debug.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/debug.cpp diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/scene.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.cpp diff --git a/planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp similarity index 100% rename from planning/behavior_velocity_planner/behavior_velocity_speed_bump_module/src/util.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/util.hpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 6075706a672d0..04ea5ca872098 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -52,7 +52,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop // Calculate stop pose and insert index const auto stop_point = arc_lane_utils::createTargetPoint( - *path, stop_line, lane_id_, planner_param_.stop_margin, + *path, stop_line, planner_param_.stop_margin, planner_data_->vehicle_info_.max_longitudinal_offset_m); // If no collision found, do nothing diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md index 5300bb3b4b5c1..0507d69af1b4e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/README.md @@ -1,6 +1,6 @@ ## Template -A template for behavior velocity modules based on the behavior_velocity_speed_bump_module. +A template for behavior velocity modules based on the autoware_behavior_velocity_speed_bump_module. # Autoware Behavior Velocity Module Template diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index d27cc23223630..1bf26d9f95c6a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -379,7 +379,7 @@ std::optional VirtualTrafficLightModule::getPathIndexOfFirstEndLine() end_line_p2.y = end_line.back().y(); const auto collision = - arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2, lane_id_); + arc_lane_utils::findCollisionSegment(module_data_.path, end_line_p1, end_line_p2); if (!collision) { continue; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index fde54a7041672..12cb59ac46195 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -48,6 +49,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -179,6 +184,16 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.ego_footprints = ego_data.trajectory_footprints; debug_data_.obstacle_footprints = obstacle_forward_footprints; debug_data_.z = ego_data.pose.position.z; + std::map processing_times; + processing_times["preprocessing"] = preprocessing_duration_us / 1000; + processing_times["footprints"] = footprints_duration_us / 1000; + processing_times["collisions"] = collisions_duration_us / 1000; + processing_times["Total"] = total_time_us / 1000; + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index aeeaabeb9b510..af63b50bceb39 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -50,7 +50,7 @@ size_t calculateStartIndex( /// @param[in] start_idx starting index of the input trajectory /// @param[in] factor factor used for downsampling /// @return downsampled trajectory -TrajectoryPoints downsampleTrajectory( +TrajectoryPoints downsample_trajectory( const TrajectoryPoints & trajectory, const size_t start_idx, const int factor); /// @brief recalculate the steering angle of the trajectory @@ -123,16 +123,6 @@ std::vector calculate_slowd const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params, autoware::motion_utils::VirtualWalls & virtual_walls); - -/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory -/// @param[in] downsampled_trajectory downsampled trajectory -/// @param[in] trajectory input trajectory -/// @param[in] start_idx starting index of the downsampled trajectory relative to the input -/// @param[in] factor downsampling factor -/// @return input trajectory with the velocity profile of the downsampled trajectory -TrajectoryPoints copyDownsampledVelocity( - const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx, - const int factor); } // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter #endif // OBSTACLE_VELOCITY_LIMITER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 2b78a2f1495bf..efbdcdaf464e2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -34,6 +35,7 @@ #include #include +#include namespace autoware::motion_velocity_planner { @@ -52,6 +54,10 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -154,7 +160,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const auto end_idx = obstacle_velocity_limiter::calculateEndIndex( original_traj_points, start_idx, preprocessing_params_.max_length, preprocessing_params_.max_duration); - auto downsampled_traj_points = obstacle_velocity_limiter::downsampleTrajectory( + auto downsampled_traj_points = downsample_trajectory( original_traj_points, start_idx, end_idx, preprocessing_params_.downsample_factor); obstacle_velocity_limiter::ObstacleMasks obstacle_masks; const auto preprocessing_us = stopwatch.toc("preprocessing"); @@ -218,6 +224,16 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( safe_footprint_polygons, obstacle_masks, planner_data->current_odometry.pose.pose.position.z)); } + std::map processing_times; + processing_times["preprocessing"] = preprocessing_us / 1000; + processing_times["obstacles"] = obstacles_us / 1000; + processing_times["slowdowns"] = slowdowns_us / 1000; + processing_times["Total"] = total_us / 1000; + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index f4b1304d67e5e..73e05bed88c37 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp @@ -55,17 +55,6 @@ size_t calculateEndIndex( return idx; } -TrajectoryPoints downsampleTrajectory( - const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx, - const int factor) -{ - if (factor < 1) return trajectory; - TrajectoryPoints downsampled_traj; - downsampled_traj.reserve((end_idx - start_idx) / factor); - for (size_t i = start_idx; i <= end_idx; i += factor) downsampled_traj.push_back(trajectory[i]); - return downsampled_traj; -} - void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base) { auto prev_point = trajectory.front(); @@ -82,16 +71,4 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt); } } - -TrajectoryPoints copyDownsampledVelocity( - const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx, - const int factor) -{ - const auto size = std::min(downsampled_traj.size(), trajectory.size()); - for (size_t i = 0; i < size; ++i) { - trajectory[start_idx + i * factor].longitudinal_velocity_mps = - downsampled_traj[i].longitudinal_velocity_mps; - } - return trajectory; -} } // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp index cb9fe40e64907..e098ee6abe822 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp @@ -37,31 +37,11 @@ size_t calculateEndIndex( const TrajectoryPoints & trajectory, const size_t start_idx, const double max_length, const double max_duration); -/// @brief downsample a trajectory, reducing its number of points by the given factor -/// @param[in] trajectory input trajectory -/// @param[in] start_idx starting index of the input trajectory -/// @param[in] end_idx ending index of the input trajectory -/// @param[in] factor factor used for downsampling -/// @return downsampled trajectory -TrajectoryPoints downsampleTrajectory( - const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx, - const int factor); - /// @brief recalculate the steering angle of the trajectory /// @details uses the change in headings for calculation /// @param[inout] trajectory input trajectory /// @param[in] wheel_base wheel base of the vehicle void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base); - -/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory -/// @param[in] downsampled_trajectory downsampled trajectory -/// @param[in] trajectory input trajectory -/// @param[in] start_idx starting index of the downsampled trajectory relative to the input -/// @param[in] factor downsampling factor -/// @return input trajectory with the velocity profile of the downsampled trajectory -TrajectoryPoints copyDownsampledVelocity( - const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx, - const int factor); } // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter #endif // TRAJECTORY_PREPROCESSING_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index f6d81f6f036d0..8facb77932aa5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -33,6 +34,7 @@ #include +#include #include #include #include @@ -56,6 +58,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -155,9 +161,11 @@ VelocityPlanningResult OutOfLaneModule::plan( stopwatch.tic(); out_of_lane::EgoData ego_data; ego_data.pose = planner_data->current_odometry.pose.pose; - ego_data.trajectory_points = ego_trajectory_points; - ego_data.first_trajectory_idx = + const auto start_idx = autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); + ego_data.trajectory_points = + downsample_trajectory(ego_trajectory_points, start_idx, ego_trajectory_points.size(), 10); + ego_data.first_trajectory_idx = 0; ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); stopwatch.tic("calculate_trajectory_footprints"); @@ -207,7 +215,7 @@ VelocityPlanningResult OutOfLaneModule::plan( inputs.ego_data = ego_data; stopwatch.tic("filter_predicted_objects"); inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); - const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); + const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects"); inputs.route_handler = planner_data->route_handler; inputs.lanelets = other_lanelets; stopwatch.tic("calculate_decisions"); @@ -288,12 +296,26 @@ VelocityPlanningResult OutOfLaneModule::plan( "\tcalc_slowdown_points = %2.0fus\n" "\tinsert_slowdown_points = %2.0fus\n", total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us, calc_slowdown_points_us, insert_slowdown_points_us); debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(debug_data_, params_)); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + std::map processing_times; + processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000; + processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000; + processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000; + processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000; + processing_times["calculate_decision"] = calculate_decisions_us / 1000; + processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000; + processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000; + processing_times["Total"] = total_time_us / 1000; + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 535510453b8ba..9bd662af697ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -19,9 +19,11 @@ #include "velocity_planning_result.hpp" #include +#include #include #include +#include #include #include @@ -44,6 +46,8 @@ class PluginModuleInterface rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + std::shared_ptr processing_diag_publisher_; + rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; }; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp new file mode 100644 index 0000000000000..95728c4738c5b --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_ + +#include + +#include + +namespace autoware::motion_velocity_planner +{ +/// @brief downsample a trajectory, reducing its number of points by the given factor +/// @param[in] trajectory input trajectory +/// @param[in] start_idx starting index of the input trajectory +/// @param[in] end_idx ending index of the input trajectory +/// @param[in] factor factor used for downsampling +/// @return downsampled trajectory +std::vector downsample_trajectory( + const std::vector & trajectory, + const size_t start_idx, const size_t end_idx, const int factor) +{ + if (factor < 1) { + return trajectory; + } + std::vector downsampled_traj; + downsampled_traj.reserve((end_idx - start_idx) / factor + 1); + for (size_t i = start_idx; i <= end_idx; i += factor) { + downsampled_traj.push_back(trajectory[i]); + } + return downsampled_traj; +} +} // namespace autoware::motion_velocity_planner + +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index d2a3e305180d5..e428719f6e32f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -27,6 +27,7 @@ lanelet2_extension libboost-dev rclcpp + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index c00e0dd856200..aad25d5fb20a1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -38,6 +38,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 714b6fd948a32..6578c5cfaca65 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include #include +#include #include #include @@ -80,6 +82,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); + processing_time_publisher_ = this->create_publisher( + "~/debug/total_time/processing_time_ms", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); @@ -103,8 +107,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); } void MotionVelocityPlannerNode::on_load_plugin( @@ -252,9 +254,14 @@ void MotionVelocityPlannerNode::on_trajectory( { std::unique_lock lk(mutex_); + autoware::universe_utils::StopWatch stop_watch; + std::map processing_times; + stop_watch.tic("Total"); + if (!update_planner_data()) { return; } + processing_times["update_planner_data"] = stop_watch.toc(true); if (input_trajectory_msg->points.empty()) { RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); @@ -264,6 +271,7 @@ void MotionVelocityPlannerNode::on_trajectory( if (has_received_map_) { planner_data_.route_handler = std::make_shared(*map_ptr_); has_received_map_ = false; + processing_times["make_RouteHandler"] = stop_watch.toc(true); } autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ @@ -271,12 +279,19 @@ void MotionVelocityPlannerNode::on_trajectory( auto output_trajectory_msg = generate_trajectory(input_trajectory_points); output_trajectory_msg.header = input_trajectory_msg->header; + processing_times["generate_trajectory"] = stop_watch.toc(true); lk.unlock(); trajectory_pub_->publish(output_trajectory_msg); - published_time_publisher_->publish_if_subscribed( + published_time_publisher_.publish_if_subscribed( trajectory_pub_, output_trajectory_msg.header.stamp); + processing_times["Total"] = stop_watch.toc("Total"); + processing_diag_publisher_.publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); } void MotionVelocityPlannerNode::insert_stop( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 7b771d0b04442..8debb9cbedf00 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -96,6 +97,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + rclcpp::Publisher::SharedPtr processing_time_publisher_; + autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; // parameters rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; @@ -139,8 +143,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index e9058f234e98f..1ab368b6f2a8d 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -639,14 +639,11 @@ double AdaptiveCruiseController::calcTargetVelocity_D( return 0.0; } - double add_vel_d = 0; - - add_vel_d = diff_vel; + double add_vel_d = diff_vel; if (add_vel_d >= 0) { - diff_vel *= param_.d_coeff_pos; - } - if (add_vel_d < 0) { - diff_vel *= param_.d_coeff_neg; + add_vel_d *= param_.d_coeff_pos; + } else { + add_vel_d *= param_.d_coeff_neg; } add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm); diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 049063a72cd5c..7a3b40336ff3d 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -61,7 +61,7 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) "gnss_pose_cov", rclcpp::QoS{1}); fixed_pub_ = create_publisher("gnss_fixed", rclcpp::QoS{1}); - // Set msg_gnss_ins_orientation_stamped_ with temoporary values (not to publish zero value + // Set msg_gnss_ins_orientation_stamped_ with temporary values (not to publish zero value // covariances) msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_x = 1.0; msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_y = 1.0; diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp index d53d12afcc499..e0009d5bdc382 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/default_ad_api/src/interface.cpp @@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf { const auto on_interface_version = [](auto, auto res) { res->major = 1; - res->minor = 3; + res->minor = 4; res->patch = 0; }; diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index d4b430ecd0579..510788dd09d08 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -66,8 +66,13 @@ void OperationModeNode::change_mode( const ResponseT res, const OperationModeRequest::_mode_type mode) { if (!mode_available_[mode]) { + RCLCPP_WARN( + get_logger(), + "The target mode is not available. Please check the cause with " + "rqt_diagnostics_graph_monitor"); throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change is blocked by the system."); + ServiceResponse::ERROR_NOT_AVAILABLE, + "The target mode is not available. Please check the diagnostics."); } const auto req = std::make_shared(); req->mode = mode; diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py index 63a79e8722be3..10d00d1dbad30 100644 --- a/system/default_ad_api/test/node/interface_version.py +++ b/system/default_ad_api/test/node/interface_version.py @@ -30,7 +30,7 @@ if response.major != 1: exit(1) -if response.minor != 3: +if response.minor != 4: exit(1) if response.patch != 0: exit(1) diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp index c9d281a5c886e..553c21b5ff4d7 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/config.cpp @@ -102,8 +102,8 @@ UnitConfig * FileLoader::create_unit_config(const TreeData & data) unit->item = create_link_config(item, unit); } const auto list = unit->data.optional("list").children(); - for (const auto & data : list) { - unit->list.push_back(create_link_config(data, unit)); + for (const auto & tree_data : list) { + unit->list.push_back(create_link_config(tree_data, unit)); } return unit; } diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt index b68e7bedb57ed..0c36964f49237 100644 --- a/system/diagnostic_graph_utils/CMakeLists.txt +++ b/system/diagnostic_graph_utils/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ament_auto_add_library(${PROJECT_NAME}_tools SHARED src/node/dump.cpp src/node/converter.cpp + src/node/logging.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_tools @@ -24,4 +25,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_tools EXECUTABLE converter_node ) -ament_auto_package() +rclcpp_components_register_node(${PROJECT_NAME}_tools + PLUGIN "diagnostic_graph_utils::LoggingNode" + EXECUTABLE logging_node +) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/diagnostic_graph_utils/launch/logging.launch.xml b/system/diagnostic_graph_utils/launch/logging.launch.xml new file mode 100644 index 0000000000000..12636574eb8be --- /dev/null +++ b/system/diagnostic_graph_utils/launch/logging.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/diagnostic_graph_utils/src/node/logging.cpp new file mode 100644 index 0000000000000..009af51e949cd --- /dev/null +++ b/system/diagnostic_graph_utils/src/node/logging.cpp @@ -0,0 +1,95 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "logging.hpp" + +#include +#include +#include +#include +#include +#include + +namespace diagnostic_graph_utils +{ + +LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging", options) +{ + root_path_ = declare_parameter("root_path"); + max_depth_ = declare_parameter("max_depth"); + + using std::placeholders::_1; + sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1)); + sub_graph_.subscribe(*this, 1); + + const auto period = rclcpp::Rate(declare_parameter("show_rate")).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, [this]() { on_timer(); }); +} + +void LoggingNode::on_create(DiagGraph::ConstSharedPtr graph) +{ + // Search root node. + root_unit_ = nullptr; + for (const auto & unit : graph->units()) { + if (unit->path() == root_path_) { + root_unit_ = unit; + return; + } + } + RCLCPP_ERROR_STREAM(get_logger(), "root unit is not found: " << root_path_); +} + +void LoggingNode::on_timer() +{ + static const auto message = "The target mode is not available for the following reasons:"; + if (root_unit_ && root_unit_->level() != DiagUnit::DiagnosticStatus::OK) { + dump_text_.str(""); + dump_text_.clear(std::stringstream::goodbit); + dump_unit(root_unit_, 0, " "); + RCLCPP_WARN_STREAM(get_logger(), message << std::endl << dump_text_.str()); + } +} + +void LoggingNode::dump_unit(DiagUnit * unit, int depth, const std::string & indent) +{ + const auto text_level = [](DiagUnit::DiagnosticLevel level) { + if (level == DiagUnit::DiagnosticStatus::OK) return "OK "; + if (level == DiagUnit::DiagnosticStatus::WARN) return "WARN "; + if (level == DiagUnit::DiagnosticStatus::ERROR) return "ERROR"; + if (level == DiagUnit::DiagnosticStatus::STALE) return "STALE"; + return "-----"; + }; + + if (max_depth_ < depth) { + return; + } + if (unit->level() == DiagUnit::DiagnosticStatus::OK) { + return; + } + + std::string path = unit->path(); + if (path.empty()) { + path = "[anonymous group]"; + } + + dump_text_ << indent << "- " + path << " " << text_level(unit->level()) << std::endl; + for (const auto & child : unit->children()) { + dump_unit(child.unit, depth + 1, indent + " "); + } +} + +} // namespace diagnostic_graph_utils + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::LoggingNode) diff --git a/system/diagnostic_graph_utils/src/node/logging.hpp b/system/diagnostic_graph_utils/src/node/logging.hpp new file mode 100644 index 0000000000000..b2d305c81ca99 --- /dev/null +++ b/system/diagnostic_graph_utils/src/node/logging.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NODE__LOGGING_HPP_ +#define NODE__LOGGING_HPP_ + +#include "diagnostic_graph_utils/subscription.hpp" + +#include + +#include +#include + +namespace diagnostic_graph_utils +{ + +class LoggingNode : public rclcpp::Node +{ +public: + explicit LoggingNode(const rclcpp::NodeOptions & options); + +private: + void on_create(DiagGraph::ConstSharedPtr graph); + void on_timer(); + void dump_unit(DiagUnit * unit, int depth, const std::string & indent); + DiagGraphSubscription sub_graph_; + rclcpp::TimerBase::SharedPtr timer_; + + DiagUnit * root_unit_; + int max_depth_; + std::string root_path_; + std::ostringstream dump_text_; +}; + +} // namespace diagnostic_graph_utils + +#endif // NODE__LOGGING_HPP_ diff --git a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp index e951f11e49ab7..923fc53055475 100644 --- a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp @@ -50,6 +50,9 @@ struct VehicleInfo autoware::universe_utils::LinearRing2d createFootprint(const double margin = 0.0) const; autoware::universe_utils::LinearRing2d createFootprint( const double lat_margin, const double lon_margin) const; + + double calcMaxCurvature() const; + double calcCurvatureFromSteerAngle(const double steer_angle) const; }; /// Create vehicle info from base parameters diff --git a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp index db223663f1145..2dd5b19f2f523 100644 --- a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -85,4 +85,30 @@ VehicleInfo createVehicleInfo( }; } +double VehicleInfo::calcMaxCurvature() const +{ + if (std::abs(wheel_base_m) < 1e-6) { + throw std::invalid_argument("wheel_base_m is 0."); + } + if (std::abs(max_steer_angle_rad) < 1e-6) { + return std::numeric_limits::max(); + } + + const double radius = wheel_base_m / std::tan(max_steer_angle_rad); + const double curvature = 1.0 / radius; + return curvature; +} +double VehicleInfo::calcCurvatureFromSteerAngle(const double steer_angle) const +{ + if (std::abs(wheel_base_m) < 1e-6) { + throw std::invalid_argument("wheel_base_m is 0."); + } + if (std::abs(steer_angle) < 1e-6) { + return std::numeric_limits::max(); + } + + const double radius = wheel_base_m / std::tan(steer_angle); + const double curvature = 1.0 / radius; + return curvature; +} } // namespace autoware::vehicle_info_utils