diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml new file mode 100644 index 0000000000000..ebd79a1dd6d92 --- /dev/null +++ b/.github/sync-files.yaml @@ -0,0 +1,33 @@ +- repository: autowarefoundation/autoware + files: + - source: CODE_OF_CONDUCT.md + - source: CONTRIBUTING.md + - source: DISCLAIMER.md + - source: LICENSE + - source: .github/dependabot.yaml + - source: .github/workflows/pre-commit-optional.yaml + - source: .github/workflows/semantic-pull-request.yaml + - source: .github/workflows/spell-check-differential.yaml + - source: .clang-format + - source: .markdown-link-check.json + - source: .markdownlint.yaml + - source: .pre-commit-config-optional.yaml + - source: .prettierignore + - source: .prettierrc.yaml + - source: .yamllint.yaml + - source: CPPLINT.cfg + - source: setup.cfg + +- repository: autowarefoundation/autoware_common + files: + - source: .github/workflows/build-and-test.yaml + - source: .github/workflows/build-and-test-differential.yaml + - source: .github/workflows/build-and-test-differential-self-hosted.yaml + - source: .github/workflows/build-and-test-self-hosted.yaml + - source: .github/workflows/check-build-depends.yaml + - source: .github/workflows/pre-commit.yaml + +- repository: autowarefoundation/autoware-documentation + files: + - source: .github/workflows/deploy-docs.yaml + - source: .github/workflows/delete-closed-pr-docs.yaml diff --git a/.github/workflows/build-and-test-arm-pr.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml similarity index 87% rename from .github/workflows/build-and-test-arm-pr.yaml rename to .github/workflows/build-and-test-differential-self-hosted.yaml index 4123734f81c2a..8d6abbaf5950a 100644 --- a/.github/workflows/build-and-test-arm-pr.yaml +++ b/.github/workflows/build-and-test-differential-self-hosted.yaml @@ -1,4 +1,4 @@ -name: build-and-test-arm-pr +name: build-and-test-differential-self-hosted on: pull_request: @@ -14,13 +14,13 @@ jobs: with: label: ARM64 - build-and-test-arm: + build-and-test-differential-self-hosted: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} runs-on: [self-hosted, linux, ARM64] - container: ros:galactic + container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 with: fetch-depth: 0 diff --git a/.github/workflows/build-and-test-pr.yaml b/.github/workflows/build-and-test-differential.yaml similarity index 72% rename from .github/workflows/build-and-test-pr.yaml rename to .github/workflows/build-and-test-differential.yaml index b97c8cc1cb803..1ba8540a1860d 100644 --- a/.github/workflows/build-and-test-pr.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -1,17 +1,17 @@ -name: build-and-test-pr +name: build-and-test-differential on: pull_request: jobs: - build-and-test: + build-and-test-differential: runs-on: ubuntu-latest - container: ros:galactic + container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.9.1 - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 with: fetch-depth: 0 @@ -36,12 +36,29 @@ jobs: target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: build_depends.repos - clang-tidy: + - name: Check the existence of coverage files + id: check-file-existence + uses: autowarefoundation/autoware-github-actions/check-file-existence@tier4/proposal + with: + files: | + lcov/total_coverage.info + coveragepy/.coverage + condition: or + + - name: Upload coverage to CodeCov + if: ${{ steps.check-file-existence.outputs.exists == 'true' }} + uses: codecov/codecov-action@v2 + with: + files: lcov/total_coverage.info,coveragepy/.coverage + fail_ci_if_error: false + verbose: true + + clang-tidy-differential: runs-on: ubuntu-latest container: ros:galactic - needs: build-and-test + needs: build-and-test-differential steps: - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 with: fetch-depth: 0 diff --git a/.github/workflows/build-and-test-scheduled.yaml b/.github/workflows/build-and-test-self-hosted.yaml similarity index 79% rename from .github/workflows/build-and-test-scheduled.yaml rename to .github/workflows/build-and-test-self-hosted.yaml index 3c2e892e4e3ce..3d28151b1d691 100644 --- a/.github/workflows/build-and-test-scheduled.yaml +++ b/.github/workflows/build-and-test-self-hosted.yaml @@ -1,4 +1,4 @@ -name: build-and-test-scheduled +name: build-and-test-self-hosted on: schedule: @@ -6,15 +6,11 @@ on: workflow_dispatch: jobs: - build-and-test-scheduled: - runs-on: ${{ matrix.os }} - container: ros:galactic - strategy: - fail-fast: false - matrix: - os: [ubuntu-latest, ARM64] + build-and-test-self-hosted: + runs-on: [self-hosted, linux, ARM64] + container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 - name: Remove exec_depend diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml new file mode 100644 index 0000000000000..a9ee40cb8e09f --- /dev/null +++ b/.github/workflows/build-and-test.yaml @@ -0,0 +1,52 @@ +name: build-and-test + +on: + push: + schedule: + - cron: 0 19 * * * # run at 4 AM JST + workflow_dispatch: + +jobs: + build-and-test: + if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware-universe:latest + steps: + - name: Check out repository + uses: actions/checkout@v2 + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + + - name: Register AutonomouStuff repository + uses: autowarefoundation/autoware-github-actions/register-autonomoustuff-repository@tier4/proposal + with: + rosdistro: galactic + + - name: Get self packages + id: get-self-packages + uses: autowarefoundation/autoware-github-actions/get-self-packages@tier4/proposal + + - name: Build and test + uses: autowarefoundation/autoware-github-actions/colcon-build-and-test@tier4/proposal + with: + rosdistro: galactic + target-packages: ${{ steps.get-self-packages.outputs.self-packages }} + build-depends-repos: build_depends.repos + + - name: Check the existence of coverage files + id: check-file-existence + uses: autowarefoundation/autoware-github-actions/check-file-existence@tier4/proposal + with: + files: | + lcov/total_coverage.info + coveragepy/.coverage + condition: or + + - name: Upload coverage to CodeCov + if: ${{ steps.check-file-existence.outputs.exists == 'true' }} + uses: codecov/codecov-action@v2 + with: + files: lcov/total_coverage.info,coveragepy/.coverage + fail_ci_if_error: false + verbose: true diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 16538e83f4dd7..20848a436a7cd 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -8,12 +8,12 @@ on: jobs: check-build-depends: runs-on: ubuntu-latest - container: ros:galactic + container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.9.1 - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 - name: Remove exec_depend diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index de35277ed184d..bb11e4dc75e78 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -9,7 +9,7 @@ jobs: delete-closed-pr-docs: runs-on: ubuntu-latest steps: - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 with: fetch-depth: 0 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 3a4fc976f75fd..ce35cb8f76ce5 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -29,7 +29,7 @@ jobs: if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} runs-on: ubuntu-latest steps: - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 with: fetch-depth: 0 diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml new file mode 100644 index 0000000000000..50b6778a25c0a --- /dev/null +++ b/.github/workflows/pre-commit-optional.yaml @@ -0,0 +1,16 @@ +name: pre-commit-optional + +on: + pull_request: + +jobs: + pre-commit-optional: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v2 + + - name: Run pre-commit + uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal + with: + pre-commit-config: .pre-commit-config-optional.yaml diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 0000000000000..9eb501a19dac9 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,16 @@ +name: pre-commit + +on: + pull_request: + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v2 + + - name: Run pre-commit + uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal + with: + pre-commit-config: .pre-commit-config.yaml diff --git a/.github/workflows/spell-check.yaml b/.github/workflows/spell-check-differential.yaml similarity index 78% rename from .github/workflows/spell-check.yaml rename to .github/workflows/spell-check-differential.yaml index 3d83a2fac7e0d..484c3690241ed 100644 --- a/.github/workflows/spell-check.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -1,13 +1,13 @@ -name: spell-check +name: spell-check-differential on: pull_request: jobs: - spell-check: + spell-check-differential: runs-on: ubuntu-latest steps: - - name: Checkout repository + - name: Check out repository uses: actions/checkout@v2 - name: Run spell-check diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml new file mode 100644 index 0000000000000..0c5b4b04fa084 --- /dev/null +++ b/.github/workflows/sync-files.yaml @@ -0,0 +1,22 @@ +name: sync-files + +on: + schedule: + - cron: 0 19 * * * # run at 4 AM JST + workflow_dispatch: + +jobs: + sync-files: + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run sync-files + uses: autowarefoundation/autoware-github-actions/sync-files@tier4/proposal + with: + token: ${{ steps.generate-token.outputs.token }} diff --git a/.markdown-link-check.json b/.markdown-link-check.json index 230751bcfe76f..331095d38dd41 100644 --- a/.markdown-link-check.json +++ b/.markdown-link-check.json @@ -1,6 +1,10 @@ { "aliveStatusCodes": [200, 206, 403], - "ignorePatterns": [], + "ignorePatterns": [ + { + "pattern": "^http://localhost" + } + ], "retryOn429": true, "retryCount": 10 } diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml new file mode 100644 index 0000000000000..e54ba1fdd5fdc --- /dev/null +++ b/.pre-commit-config-optional.yaml @@ -0,0 +1,6 @@ +repos: + - repo: https://github.com/tcort/markdown-link-check + rev: v3.9.3 + hooks: + - id: markdown-link-check + args: [--config=.markdown-link-check.json] diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 22d89e2c72406..14528e5fec7c7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.0.1 + rev: v4.1.0 hooks: - id: check-json - id: check-merge-conflict @@ -24,7 +24,7 @@ repos: args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v2.5.0 + rev: v2.5.1 hooks: - id: prettier @@ -40,12 +40,12 @@ repos: - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.8.0.1 + rev: v0.8.0.3 hooks: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.4.1-1 + rev: v3.4.2-1 hooks: - id: shfmt args: [-w, -s, -i=4] @@ -56,7 +56,7 @@ repos: - id: isort - repo: https://github.com/psf/black - rev: 21.11b1 + rev: 22.1.0 hooks: - id: black args: [--line-length=100] @@ -78,7 +78,7 @@ repos: ] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v12.0.1 + rev: v13.0.0 hooks: - id: clang-format diff --git a/.prettierignore b/.prettierignore index f740580782527..a3c34d00a1377 100644 --- a/.prettierignore +++ b/.prettierignore @@ -1 +1,2 @@ *.param.yaml +*.rviz diff --git a/.yamllint.yaml b/.yamllint.yaml index 1d4ee33ecacc0..6228c70f02da9 100644 --- a/.yamllint.yaml +++ b/.yamllint.yaml @@ -5,6 +5,9 @@ ignore: | *.param.yaml rules: + braces: + level: error + max-spaces-inside: 1 # To format with Prettier comments: level: error min-spaces-from-content: 1 # To be compatible with C++ and Python diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index cac75e3bba195..6894cc8f6d11c 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -56,14 +56,9 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions self_pose_listener_.waitForFirstPose(); // Timer - double delta_time = 1.0 / static_cast(node_param_.update_rate); - auto timer_callback_ = std::bind(&GoalDistanceCalculatorNode::onTimer, this); - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(delta_time)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback_), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&GoalDistanceCalculatorNode::onTimer, this)); goal_distance_calculator_ = std::make_unique(); } diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp index b0c6d2ea3cb52..46bbc79f2f8cd 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -44,7 +44,7 @@ using autoware::common::types::float64_t; class OSQP_INTERFACE_PUBLIC OSQPInterface { private: - OSQPWorkspace * m_work = nullptr; + std::unique_ptr> m_work; std::unique_ptr m_settings; std::unique_ptr m_data; // store last work info since work is cleaned up at every execution to prevent memory leak. @@ -59,6 +59,8 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface // Runs the solver on the stored problem. std::tuple, std::vector, int64_t, int64_t> solve(); + static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; + public: /// \brief Constructor without problem formulation explicit OSQPInterface( @@ -73,12 +75,11 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface OSQPInterface( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u, const c_float eps_abs); - ~OSQPInterface(); /**************** * OPTIMIZATION ****************/ - /// \brief Solves the stored convec quadratic program (QP) problem using the OSQP solver. + /// \brief Solves the stored convex quadratic program (QP) problem using the OSQP solver. // /// \return The function returns a tuple containing the solution as two float vectors. /// \return The first element of the tuple contains the 'primal' solution. diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp index 16eb1f559342b..5b60dc55b4f96 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -32,6 +32,7 @@ namespace common namespace osqp { OSQPInterface::OSQPInterface(const c_float eps_abs, const bool8_t polish) +: m_work{nullptr, OSQPWorkspaceDeleter} { m_settings = std::make_unique(); m_data = std::make_unique(); @@ -59,6 +60,13 @@ OSQPInterface::OSQPInterface( initializeProblem(P, A, q, l, u); } +void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept +{ + if (ptr != nullptr) { + osqp_cleanup(ptr); + } +} + void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) { /* @@ -71,7 +79,7 @@ void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) c_int P_elem_N = P_sparse.nonZeros(); */ CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P(m_work, P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); + osqp_update_P(m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); } void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) @@ -84,7 +92,7 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) c_int A_elem_N = A_sparse.nonZeros(); */ CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A(m_work, A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + osqp_update_A(m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); return; } @@ -92,21 +100,21 @@ void OSQPInterface::updateQ(const std::vector & q_new) { std::vector q_tmp(q_new.begin(), q_new.end()); double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work, q_dyn); + osqp_update_lin_cost(m_work.get(), q_dyn); } void OSQPInterface::updateL(const std::vector & l_new) { std::vector l_tmp(l_new.begin(), l_new.end()); double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work, l_dyn); + osqp_update_lower_bound(m_work.get(), l_dyn); } void OSQPInterface::updateU(const std::vector & u_new) { std::vector u_tmp(u_new.begin(), u_new.end()); double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work, u_dyn); + osqp_update_upper_bound(m_work.get(), u_dyn); } void OSQPInterface::updateBounds( @@ -116,14 +124,14 @@ void OSQPInterface::updateBounds( std::vector u_tmp(u_new.begin(), u_new.end()); double * l_dyn = l_tmp.data(); double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work, l_dyn, u_dyn); + osqp_update_bounds(m_work.get(), l_dyn, u_dyn); } void OSQPInterface::updateEpsAbs(const double eps_abs) { m_settings->eps_abs = eps_abs; // for default setting if (m_work_initialized) { - osqp_update_eps_abs(m_work, eps_abs); // for current work + osqp_update_eps_abs(m_work.get(), eps_abs); // for current work } } @@ -131,7 +139,7 @@ void OSQPInterface::updateEpsRel(const double eps_rel) { m_settings->eps_rel = eps_rel; // for default setting if (m_work_initialized) { - osqp_update_eps_rel(m_work, eps_rel); // for current work + osqp_update_eps_rel(m_work.get(), eps_rel); // for current work } } @@ -139,7 +147,7 @@ void OSQPInterface::updateMaxIter(const int max_iter) { m_settings->max_iter = max_iter; // for default setting if (m_work_initialized) { - osqp_update_max_iter(m_work, max_iter); // for current work + osqp_update_max_iter(m_work.get(), max_iter); // for current work } } @@ -147,7 +155,7 @@ void OSQPInterface::updateVerbose(const bool is_verbose) { m_settings->verbose = is_verbose; // for default setting if (m_work_initialized) { - osqp_update_verbose(m_work, is_verbose); // for current work + osqp_update_verbose(m_work.get(), is_verbose); // for current work } } @@ -160,7 +168,7 @@ void OSQPInterface::updateRho(const double rho) { m_settings->rho = rho; if (m_work_initialized) { - osqp_update_rho(m_work, rho); + osqp_update_rho(m_work.get(), rho); } } @@ -168,7 +176,7 @@ void OSQPInterface::updateAlpha(const double alpha) { m_settings->alpha = alpha; if (m_work_initialized) { - osqp_update_alpha(m_work, alpha); + osqp_update_alpha(m_work.get(), alpha); } } @@ -213,24 +221,18 @@ int64_t OSQPInterface::initializeProblem( m_data->u = u_dyn; // Setup workspace - m_exitflag = osqp_setup(&m_work, m_data.get(), m_settings.get()); + OSQPWorkspace * workspace; + m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); + m_work.reset(workspace); m_work_initialized = true; return m_exitflag; } -OSQPInterface::~OSQPInterface() -{ - // Cleanup dynamic OSQP memory - if (m_work) { - osqp_cleanup(m_work); - } -} - std::tuple, std::vector, int64_t, int64_t> OSQPInterface::solve() { // Solve Problem - osqp_solve(m_work); + osqp_solve(m_work.get()); /******************** * EXTRACT SOLUTION @@ -271,6 +273,9 @@ OSQPInterface::optimize( // Run the solver on the stored problem representation. std::tuple, std::vector, int64_t, int64_t> result = solve(); + m_work.reset(); + m_work_initialized = false; + return result; } diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index bfc3b55c1d66a..59b04739c0a63 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -31,7 +31,8 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio pub_dist_ = create_publisher("~/output/distance", rclcpp::QoS(1)); - auto timer_callback = [this]() { + using std::chrono_literals::operator""s; + timer_ = rclcpp::create_timer(this, get_clock(), 1s, [this]() { const auto path = path_; const auto pose = self_pose_listener_.getCurrentPose(); if (!pose) { @@ -53,13 +54,7 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio msg.stamp = pose->header.stamp; msg.data = distance; pub_dist_->publish(msg); - }; - - using namespace std::literals::chrono_literals; - timer_ = std::make_shared>( - this->get_clock(), 1s, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + }); } #include diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index c4615db56590b..4ec7bbad282d9 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -26,7 +26,6 @@ #include #include -#include #include #include #include @@ -80,13 +79,6 @@ inline geometry_msgs::msg::Point getPoint( return p.pose.position; } -template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose.position; -} - template geometry_msgs::msg::Pose getPose([[maybe_unused]] const T & p) { diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp index 7e8d50dbbff94..f7b17654f2c86 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp @@ -166,6 +166,44 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point return nearest_idx; } +/** + * @brief find nearest segment index to pose + * segment is straight path between two continuous points of trajectory + * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory + * @param pose pose to which to find nearest segment index + * @param max_dist max distance to search + * @param max_yaw max yaw to search + * @return nearest index + */ +template +boost::optional findNearestSegmentIndex( + const T & points, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw); + + if (!nearest_idx) { + return boost::none; + } + + if (*nearest_idx == 0) { + return 0; + } + if (*nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, *nearest_idx, pose.position); + + if (signed_length <= 0) { + return *nearest_idx - 1; + } + + return *nearest_idx; +} + /** * @brief calculate lateral offset from p_target (length from p_target to trajectory) * If seg_idx point is after that nearest point, length is negative @@ -265,6 +303,34 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } +/** + * @brief calcSignedArcLength from pose to point + */ +template +boost::optional calcSignedArcLength( + const T & points, const geometry_msgs::msg::Pose & src_pose, + const geometry_msgs::msg::Point & dst_point, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + validateNonEmpty(points); + + const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); + if (!src_seg_idx) { + return boost::none; + } + + const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); + + const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_seg_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + /** * @brief calcArcLength for the whole length */ diff --git a/common/web_controller/launch/web_controller.launch.xml b/common/web_controller/launch/web_controller.launch.xml index b6a17ac01ec50..ced4536cf122e 100644 --- a/common/web_controller/launch/web_controller.launch.xml +++ b/common/web_controller/launch/web_controller.launch.xml @@ -1,8 +1,5 @@ - - - diff --git a/control/control_performance_analysis/CMakeLists.txt b/control/control_performance_analysis/CMakeLists.txt index 7adf34bea4eb5..6ac6c7d772c4c 100644 --- a/control/control_performance_analysis/CMakeLists.txt +++ b/control/control_performance_analysis/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 36b8311c70efd..9f051534d2972 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -88,7 +88,6 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Parameters Param param_{}; // wheelbase, control period and feedback coefficients. - TargetPerformanceMsgVars target_error_vars_{}; // Subscriber Parameters Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj. diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 9847b874c8cb2..3a5f593818946 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -162,8 +162,6 @@ bool ControlPerformanceAnalysisCore::isDataReady() const std::pair ControlPerformanceAnalysisCore::getPerformanceVars() { - constexpr double EPS = std::numeric_limits::epsilon(); - // Check if data is ready. if (!isDataReady() || !idx_prev_wp_) { return std::make_pair(false, TargetPerformanceMsgVars{}); @@ -225,9 +223,6 @@ std::pair ControlPerformanceAnalysisCore::getPer target_vars.error_energy = error_vec.dot(error_vec); target_vars.value_approximation = error_vec.transpose() * lyap_P_ * error_vec; // x'Px - double prev_value_approximation = prev_target_vars_->value_approximation; - double && value_decay = target_vars.value_approximation - prev_value_approximation; - target_vars.curvature_estimate = curvature_est; target_vars.curvature_estimate_pp = curvature_est_pp; @@ -401,8 +396,6 @@ double ControlPerformanceAnalysisCore::estimateCurvature() front_axleWP_pose_prev.position.x - front_axleWP_pose.position.x, front_axleWP_pose_prev.position.y - front_axleWP_pose.position.y); - auto distance_to_traj0 = idx_prev_waypoint * ds_arc_length; - // Define waypoints 10 meters behind the rear axle if exist. // If not exist, we will take the first point of the // curvature triangle as the start point of the trajectory. diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 2a48ca4971ce9..7914f9ee24e10 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -86,13 +86,10 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( // Timer { - auto on_timer = std::bind(&ControlPerformanceAnalysisNode::onTimer, this); - auto period = std::chrono::duration_cast( + const auto period_ns = std::chrono::duration_cast( std::chrono::duration(param_.control_period)); - timer_publish_ = std::make_shared>( - this->get_clock(), period, std::move(on_timer), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_publish_, nullptr); + timer_publish_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ControlPerformanceAnalysisNode::onTimer, this)); } // Wait for first self pose diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp index cd2175f714204..69e41f11e628e 100644 --- a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp @@ -101,12 +101,9 @@ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_option }); // Timer - auto timer_callback = std::bind(&ExternalCmdSelector::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / update_rate)); - timer_ = std::make_shared>( - get_clock(), period, std::move(timer_callback), get_node_base_interface()->get_context()); - get_node_timers_interface()->add_timer(timer_, callback_group_subscribers_); + const auto period_ns = rclcpp::Rate(update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ExternalCmdSelector::onTimer, this)); } void ExternalCmdSelector::onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg) diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index f7e3f656e903e..5d61be904f845 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -437,13 +437,10 @@ void AutowareJoyControllerNode::publishVehicleEngage() void AutowareJoyControllerNode::initTimer(double period_s) { - auto timer_callback = std::bind(&AutowareJoyControllerNode::onTimer, this); const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&AutowareJoyControllerNode::onTimer, this)); } AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options) diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index dc14871da47fa..3509a5c83294f 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -173,14 +173,9 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o self_pose_listener_.waitForFirstPose(); // Timer - double delta_time = 1.0 / static_cast(node_param_.update_rate); - auto timer_callback_ = std::bind(&LaneDepartureCheckerNode::onTimer, this); - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(delta_time)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback_), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this)); } void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index 2afb81453db26..30b51941dda78 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -124,13 +124,10 @@ void ObstacleCollisionCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedP void ObstacleCollisionCheckerNode::initTimer(double period_s) { - auto timer_callback = std::bind(&ObstacleCollisionCheckerNode::onTimer, this); const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ObstacleCollisionCheckerNode::onTimer, this)); } bool ObstacleCollisionCheckerNode::isDataReady() diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp index aed71503290bc..59a89c9159672 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -89,13 +89,10 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) // Timer { - auto timer_callback = std::bind(&PurePursuitNode::onTimer, this); - auto period = std::chrono::duration_cast( + const auto period_ns = std::chrono::duration_cast( std::chrono::duration(param_.ctrl_period)); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PurePursuitNode::onTimer, this)); } // Wait for first current pose diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index dc7fe6e8fed61..36a574c790e93 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -69,13 +69,10 @@ void ShiftDecider::updateCurrentShiftCmd() void ShiftDecider::initTimer(double period_s) { - auto timer_callback = std::bind(&ShiftDecider::onTimer, this); const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = + rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&ShiftDecider::onTimer, this)); } #include diff --git a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp b/control/trajectory_follower_nodes/src/lateral_controller_node.cpp index 52b818ea2e3c3..7569f1c2a8709 100644 --- a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/lateral_controller_node.cpp @@ -395,14 +395,10 @@ const void LateralController::initTimer(float64_t period_s) { - auto timer_callback = std::bind(&LateralController::onTimer, this); const auto period_ns = std::chrono::duration_cast( - std::chrono::duration( - period_s)); - m_timer = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(m_timer, nullptr); + std::chrono::duration(period_s)); + m_timer = rclcpp::create_timer(this, get_clock(), period_ns, + std::bind(&LateralController::onTimer, this)); } void LateralController::declareMPCparameters() diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index edb52345bc07d..39f872a479985 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -188,13 +188,9 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_ // Timer { - auto timer_callback = std::bind(&LongitudinalController::callbackTimerControl, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / m_control_rate)); - m_timer_control = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(m_timer_control, nullptr); + const auto period_ns = rclcpp::Rate(m_control_rate).period(); + m_timer_control = rclcpp::create_timer(this, get_clock(), period_ns, + std::bind(&LongitudinalController::callbackTimerControl, this)); } // set parameter callback @@ -532,12 +528,16 @@ LongitudinalController::ControlState LongitudinalController::updateControlState( if (departure_condition_from_stopping) { m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); + // prevent the car from taking a long time to start to move + m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); return ControlState::DRIVE; } } else if (current_control_state == ControlState::STOPPED) { if (departure_condition_from_stopped) { m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); + // prevent the car from taking a long time to start to move + m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); return ControlState::DRIVE; } } else if (m_control_state == ControlState::EMERGENCY) { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index a3cd7d2219ef0..d455a6be73867 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -189,13 +189,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) start_request_ = std::make_unique(this, use_start_request); // Timer - auto timer_callback = std::bind(&VehicleCmdGate::onTimer, this); - auto period = std::chrono::duration_cast( + const auto period_ns = std::chrono::duration_cast( std::chrono::duration(update_period_)); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = + rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&VehicleCmdGate::onTimer, this)); } bool VehicleCmdGate::isHeartbeatTimeout( @@ -517,7 +514,7 @@ autoware_auto_control_msgs::msg::AckermannControlCommand VehicleCmdGate::createS const { autoware_auto_control_msgs::msg::AckermannControlCommand cmd; - + cmd.stamp = this->now(); cmd.lateral.steering_tire_angle = current_steer_; cmd.lateral.steering_tire_rotation_rate = 0.0; cmd.longitudinal.speed = 0.0; diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml index d04b872f1476f..ae5a39dd99ceb 100644 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml @@ -26,6 +26,9 @@ # Threshold for deciding whether to trust the estimation result converged_param_transform_probability: 3.0 + # The number of particles to estimate initial pose + initial_estimate_particles_num: 100 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index bfe310b73725f..4ddd16a8f7d1e 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -7,11 +7,15 @@ detection_area_right_expand_dist: 0.0 detection_area_left_expand_dist: 1.0 + enable_avoidance_over_same_direction: true + enable_avoidance_over_opposite_direction: true + threshold_distance_object_is_on_center: 1.0 # [m] threshold_speed_object_is_stopped: 1.0 # [m/s] object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] - lateral_collision_margin: 2.0 # [m] + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] @@ -20,6 +24,8 @@ min_nominal_avoidance_speed: 7.0 # [m/s] min_sharp_avoidance_speed: 1.0 # [m/s] + road_shoulder_safety_margin: 0.5 # [m] + max_right_shift_length: 5.0 max_left_shift_length: 5.0 diff --git a/launch/tier4_vehicle_launch/README.md b/launch/tier4_vehicle_launch/README.md index d2fba1fc665c2..c2d0dcbb8a3e5 100644 --- a/launch/tier4_vehicle_launch/README.md +++ b/launch/tier4_vehicle_launch/README.md @@ -2,7 +2,7 @@ ## Structure -![tier4_vehicle_launch](./tier4_vehicle_launch.drawio.svg) +![tier4_vehicle_launch](./vehicle_launch.drawio.svg) ## Package Dependencies diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 183a1fa35f019..08b0f4b4f33b3 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -71,21 +71,14 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c * ekf_dt_, 2.0); /* initialize ros system */ - auto timer_control_callback = std::bind(&EKFLocalizer::timerCallback, this); - auto period_control = + auto period_control_ns = std::chrono::duration_cast(std::chrono::duration(ekf_dt_)); - timer_control_ = std::make_shared>( - get_clock(), period_control, std::move(timer_control_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_control_, nullptr); - - auto timer_tf_callback = std::bind(&EKFLocalizer::timerTFCallback, this); - auto period_tf = std::chrono::duration_cast( - std::chrono::duration(1.0 / tf_rate_)); - timer_tf_ = std::make_shared>( - get_clock(), period_tf, std::move(timer_tf_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_tf_, nullptr); + timer_control_ = rclcpp::create_timer( + this, get_clock(), period_control_ns, std::bind(&EKFLocalizer::timerCallback, this)); + + const auto period_tf_ns = rclcpp::Rate(tf_rate_).period(); + timer_tf_ = rclcpp::create_timer( + this, get_clock(), period_tf_ns, std::bind(&EKFLocalizer::timerTFCallback, this)); pub_pose_ = create_publisher("ekf_pose", 1); pub_pose_cov_ = diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp index 1d4260859bb22..ceb9d520a75b0 100644 --- a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp +++ b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp @@ -48,12 +48,9 @@ class TestEKFLocalizerNode : public EKFLocalizer sub_pose = this->create_subscription( "/ekf_pose", 1, std::bind(&TestEKFLocalizerNode::testCallbackPose, this, _1)); - auto test_timer_callback = std::bind(&TestEKFLocalizerNode::testTimerCallback, this); - auto period = std::chrono::milliseconds(100); - test_timer_ = std::make_shared>( - this->get_clock(), period, std::move(test_timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(test_timer_, nullptr); + using std::chrono_literals::operator""ms; + test_timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&TestEKFLocalizerNode::testTimerCallback, this)); } ~TestEKFLocalizerNode() {} diff --git a/localization/initial_pose_button_panel/CMakeLists.txt b/localization/initial_pose_button_panel/CMakeLists.txt index 6549261a0025d..b4194c5daf792 100644 --- a/localization/initial_pose_button_panel/CMakeLists.txt +++ b/localization/initial_pose_button_panel/CMakeLists.txt @@ -5,7 +5,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp index 1de861edc2a58..114883e33da58 100644 --- a/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp +++ b/localization/initial_pose_button_panel/src/initial_pose_button_panel.cpp @@ -105,9 +105,8 @@ void InitialPoseButtonPanel::pushInitializeButton() req->pose_with_covariance = pose_cov_msg_; client_->async_send_request( - req, - [this](rclcpp::Client::SharedFuture - result) { + req, [this]([[maybe_unused]] rclcpp::Client< + tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedFuture result) { status_label_->setStyleSheet("QLabel { background-color : lightgreen;}"); status_label_->setText("OK!!!"); diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index cb42896164b9e..d3cbdbeb4347c 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -55,13 +55,9 @@ LocalizationErrorMonitor::LocalizationErrorMonitor() &LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection); // Set timer - auto timer_callback = std::bind(&LocalizationErrorMonitor::onTimer, this); - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(0.1)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&LocalizationErrorMonitor::onTimer, this)); } void LocalizationErrorMonitor::onTimer() { updater_.force_update(); } diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index b784a35edc516..9b044ec409934 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -27,6 +27,9 @@ # Threshold for deciding whether to trust the estimation result converged_param_transform_probability: 3.0 + # The number of particles to estimate initial pose + initial_estimate_particles_num: 100 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 3c01ad55ed006..f4407819a0ae0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -153,6 +153,7 @@ class NDTScanMatcher : public rclcpp::Node std::string ndt_base_frame_; std::string map_frame_; double converged_param_transform_probability_; + int initial_estimate_particles_num_; float inversion_vector_threshold_; float oscillation_threshold_; std::array output_pose_covariance_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp index 9410a54a44eef..85fecf11f060c 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp @@ -65,8 +65,7 @@ void popOldPose( Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose); std::vector createRandomPoseArray( - const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, - const size_t particle_num); + const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num); template T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index eb925abd544aa..855b7c9750623 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -103,6 +103,7 @@ NDTScanMatcher::NDTScanMatcher() ndt_base_frame_("ndt_base_link"), map_frame_("map"), converged_param_transform_probability_(4.5), + initial_estimate_particles_num_(100), inversion_vector_threshold_(-0.9), oscillation_threshold_(10) { @@ -165,6 +166,9 @@ NDTScanMatcher::NDTScanMatcher() converged_param_transform_probability_ = this->declare_parameter( "converged_param_transform_probability", converged_param_transform_probability_); + initial_estimate_particles_num_ = + this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_); + std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { @@ -602,7 +606,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar } // generateParticle - const auto initial_poses = createRandomPoseArray(initial_pose_with_cov, 100); + const auto initial_poses = + createRandomPoseArray(initial_pose_with_cov, initial_estimate_particles_num_); std::vector particle_array; auto output_cloud = std::make_shared>(); diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index d2c41f47cd27a..86d994b7c2361 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -213,8 +213,7 @@ Eigen::Affine3d fromRosPoseToEigen(const geometry_msgs::msg::Pose & ros_pose) } std::vector createRandomPoseArray( - const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, - const size_t particle_num) + const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num) { std::default_random_engine engine(seed_gen()); const Eigen::Map covariance = @@ -230,7 +229,7 @@ std::vector createRandomPoseArray( const auto base_rpy = getRPY(base_pose_with_cov); std::vector poses; - for (size_t i = 0; i < particle_num; ++i) { + for (int i = 0; i < particle_num; ++i) { geometry_msgs::msg::Vector3 xyz; geometry_msgs::msg::Vector3 rpy; diff --git a/localization/vehicle_velocity_converter/CMakeLists.txt b/localization/vehicle_velocity_converter/CMakeLists.txt index e2c108c244538..b63c8f42be84a 100644 --- a/localization/vehicle_velocity_converter/CMakeLists.txt +++ b/localization/vehicle_velocity_converter/CMakeLists.txt @@ -8,8 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - # add_compile_options(-Wall -Wextra -Wpedantic) - add_compile_options(-Wno-unused-parameter) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() # find dependencies diff --git a/localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 997b98a6bbcf4..d4cc91a2b0ae9 100644 --- a/localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -17,8 +17,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_converter") { // set covariance value for twist with covariance msg - declare_parameter("twist_covariance"); - std::vector covariance = get_parameter("twist_covariance").as_double_array(); + std::vector covariance = declare_parameter>("twist_covariance"); for (std::size_t i = 0; i < covariance.size(); ++i) { twist_covariance_[i] = covariance[i]; } diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index d3e2f3b07d7dd..cbf4630e201e4 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -5,7 +5,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() find_package(ament_cmake_auto REQUIRED) @@ -15,6 +15,7 @@ ament_auto_find_build_dependencies() include_directories( include + SYSTEM ${PCL_COMMON_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index d24698f7d181e..8c386f1bb2f44 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/perception/map_based_prediction/include/map_based_prediction.hpp b/perception/map_based_prediction/include/map_based_prediction.hpp index e6831c2be30f8..e297f51ae357d 100644 --- a/perception/map_based_prediction/include/map_based_prediction.hpp +++ b/perception/map_based_prediction/include/map_based_prediction.hpp @@ -48,11 +48,11 @@ class MapBasedPrediction const double height, const double current_d_position, const double current_d_velocity, const double current_s_position, const double current_s_velocity, const std_msgs::msg::Header & origin_header, Spline2D & spline2d, - autoware_auto_perception_msgs::msg::PredictedPath & path); + autoware_auto_perception_msgs::msg::PredictedPath & path) const; void getLinearPredictedPath( const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path); + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const; void normalizeLikelihood( autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics); diff --git a/perception/map_based_prediction/src/map_based_prediction.cpp b/perception/map_based_prediction/src/map_based_prediction.cpp index a816f780c5214..e289bc0e8b6d0 100644 --- a/perception/map_based_prediction/src/map_based_prediction.cpp +++ b/perception/map_based_prediction/src/map_based_prediction.cpp @@ -192,8 +192,8 @@ void MapBasedPrediction::normalizeLikelihood( bool MapBasedPrediction::getPredictedPath( const double height, const double current_d_position, const double current_d_velocity, const double current_s_position, const double current_s_velocity, - const std_msgs::msg::Header & origin_header, Spline2D & spline2d, - autoware_auto_perception_msgs::msg::PredictedPath & path) + [[maybe_unused]] const std_msgs::msg::Header & origin_header, Spline2D & spline2d, + autoware_auto_perception_msgs::msg::PredictedPath & path) const { // Quintic polynomial for d // A = np.array([[T**3, T**4, T**5], @@ -269,7 +269,7 @@ bool MapBasedPrediction::getPredictedPath( void MapBasedPrediction::getLinearPredictedPath( const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const { const double & sampling_delta_time = sampling_delta_time_; const double & time_horizon = time_horizon_; diff --git a/perception/map_based_prediction/src/map_based_prediction_ros.cpp b/perception/map_based_prediction/src/map_based_prediction_ros.cpp index f4cfdc32dae3a..4b77aabb818b6 100644 --- a/perception/map_based_prediction/src/map_based_prediction_ros.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_ros.cpp @@ -557,7 +557,7 @@ void MapBasedPredictionROS::objectsCallback( "map", // src rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(get_logger(), ex.what()); + RCLCPP_ERROR(get_logger(), "%s", ex.what()); return; } diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index e6111384f1476..cef4b3857d372 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -114,9 +114,9 @@ Execution time for varying the sparsity with matrix size 100. This package makes use of external code. -| Name | License | Original Repository | -| -------------------------------------------------------------- | ------------------------------------------------------------------- | ------------------------------------ | -| [muSSP](src/data_association/mu_successive_shortest_path/impl) | [Apache 2.0](src/data_association/mu_successive_shortest_path/impl) | | +| Name | License | Original Repository | +| --------------------------------------------------------- | --------------------------------------------------------- | ------------------------------------ | +| [muSSP](src/data_association/mu_successive_shortest_path) | [Apache-2.0](https://www.apache.org/licenses/LICENSE-2.0) | | [1] C. Wang, Y. Wang, Y. Wang, C.-t. Wu, and G. Yu, “muSSP: Efficient Min-cost Flow Algorithm for Multi-object Tracking,” NeurIPS, 2019 diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d9f0d3ff44428..6ec99f44fa6ce 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -182,14 +182,9 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Create ROS time based timer if (enable_delay_compensation) { - auto timer_callback = std::bind(&MultiObjectTracker::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / publish_rate)); - - publish_timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(publish_timer_, nullptr); + const auto period_ns = rclcpp::Rate(publish_rate).period(); + publish_timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this)); } const auto tmp = this->declare_parameter>("can_assign_matrix"); diff --git a/perception/tensorrt_yolo/src/nodelet.cpp b/perception/tensorrt_yolo/src/nodelet.cpp index b2bf1e41af793..85caeebc02d00 100644 --- a/perception/tensorrt_yolo/src/nodelet.cpp +++ b/perception/tensorrt_yolo/src/nodelet.cpp @@ -90,14 +90,9 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) new yolo::Net(onnx_file, mode, 1, yolo_config_, calibration_images, calib_cache_file)); net_ptr_->save(engine_file); } - auto timer_callback = std::bind(&TensorrtYoloNodelet::connectCb, this); - const auto period_s = 0.1; - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&TensorrtYoloNodelet::connectCb, this)); std::lock_guard lock(connect_mutex_); diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 768407be3964e..a593f34fd8273 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -40,15 +40,9 @@ TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeO this->create_publisher( "~/output/traffic_signals", rclcpp::QoS{1}); - // - auto timer_callback = std::bind(&TrafficLightClassifierNodelet::connectCb, this); - const auto period_s = 0.1; - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&TrafficLightClassifierNodelet::connectCb, this)); int classifier_type = this->declare_parameter( "classifier_type", static_cast(TrafficLightClassifierNodelet::ClassifierType::HSVFilter)); diff --git a/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp index b11b86f22194a..c6fe210165679 100644 --- a/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp @@ -84,14 +84,9 @@ TrafficLightSSDFineDetectorNodelet::TrafficLightSSDFineDetectorNodelet( mean_ = toFloatVector(this->declare_parameter("mean", std::vector({0.5, 0.5, 0.5}))); std_ = toFloatVector(this->declare_parameter("std", std::vector({0.5, 0.5, 0.5}))); - auto timer_callback = std::bind(&TrafficLightSSDFineDetectorNodelet::connectCb, this); - const auto period_s = 0.1; - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&TrafficLightSSDFineDetectorNodelet::connectCb, this)); std::lock_guard lock(connect_mutex_); output_roi_pub_ = diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index 0f2d18ae97077..5ebf4f9a61c50 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -43,14 +43,9 @@ TrafficLightRoiVisualizerNodelet::TrafficLightRoiVisualizerNodelet( std::bind(&TrafficLightRoiVisualizerNodelet::imageRoiCallback, this, _1, _2, _3)); } - auto timer_callback = std::bind(&TrafficLightRoiVisualizerNodelet::connectCb, this); - const auto period_s = 0.1; - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&TrafficLightRoiVisualizerNodelet::connectCb, this)); image_pub_ = image_transport::create_publisher(this, "~/output/image", rclcpp::QoS{1}.get_rmw_qos_profile()); diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 6a1fbe5f587c8..ed1adff4f6181 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -238,6 +238,148 @@ The shift points are modified by a filtering process in order to get the expecte - Similar gradient removal: Connect two shift points with a straight line, and remove the shift points in between if their shift amount is in the vicinity of the straight line. - Remove momentary returns: For shift points that reduce the avoidance width (for going back to the center line), if there is enough long distance in the longitudinal direction, remove them. +#### Computing shift length (available as of develop/v0.25.0) + +**Note**: This feature is available as of develop/`v0.25.0`. + +The shift length is set as a constant value before the feature is implemented (develop/`v0.24.0` and below). Setting the shift length like this will cause the module to generate an avoidance path regardless of actual environmental properties. For example, the path might exceed the actual road boundary or go towards a wall. Therefore, to address this limitation, in addition to [how to decide the target obstacle](#how-to-decide-the-target-obstacles), the upgraded module also takes into account the following additional element + +- The obstacles' current lane and position. +- The road shoulder with reference to the direction to avoid. + - Note: Lane direction is disregarded. + +These elements are used to compute the distance from the object to the road's shoulder (`(class ObjectData.to_road_shoulder_distance)`). + +![fig1](./image/obstacle_to_road_shoulder_distance.png) + +##### Computing shift length + +To compute the shift length, in addition to the vehicle's width and the parameterized `lateral_collision_margin`, the upgraded feature also adds two new parameters; `lateral_collision_safety_buffer` and `road_shoulder_safety_margin`. + +- The `lateral_collision_safety_buffer` parameter is used to set a safety gap that will act as the final line of defense when computing avoidance path. + - The rationale behind having this parameter is that the parameter `lateral_collision_margin` might be changed according to the situation for various reasons. Therefore, `lateral_collision_safety_buffer` will act as the final line of defense in case of the usage of `lateral_collision_margin` fails. + - It is recommended to set the value to more than half of the ego vehicle's width. +- The `road_shoulder_safety_margin` will prevent the module from generating a path that might cause the vehicle to go too near the road shoulder. + +![fig1](./image/shift_length_parameters.png) + +The shift length is subjected to the following constraints. + + + +$$ +\text{shift_length}=\begin{cases}d_{lcsb}+d_{lcm}+\frac{1}{2}(W_{ego})&\text{if}&(d_{lcsb}+d_{lcm}+W_{ego}+d_{rssm})\lt d_{trsd}\\0 &\textrm{if}&\left(d_{lcsb}+d_{lcm}+W_{ego}+d_{rssm}\right)\geq d_{trsd}\end{cases} +$$ + +where + +- = `lateral_collision_safety_buffer` +- = `lateral_collision_margin` +- = ego vehicle's width +- = `road_shoulder_safety_margin` +- = `(class ObjectData).to_road_shoulder_distance` + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title avoidance path planning +start +partition calcAvoidanceTargetObjects() { +:calcOverhangDistance; +note right + - get overhang_pose +end note + +:getClosestLaneletWithinRoute; +note right + - obtain overhang_lanelet +end note + +if(Is overhang_lanelet.id() exist?) then (no) +stop +else (\n yes) + +if(isOnRight(obstacle)?) then (yes) +partition getLeftMostLineString() { +repeat +repeat +:getLeftLanelet; +note left + Check both + - lane-changeable lane + - non lane-changeable lane +end note +repeat while (Same direction Lanelet exist?) is (yes) not (no) +:getLeftOppositeLanelet; +repeat while (Opposite direction Lanelet exist?) is (yes) not (no) +} +:compute\n(class ObjectData).to_road_shoulder_distance; +note left +distance from overhang_pose +to left most linestring +end note +else(\n no) +partition getrightMostLineString() { +repeat +repeat +:getRightLanelet; +note right + Check both + - lane-changeable lane + - non lane-changeable lane +end note +repeat while (Same direction Lanelet exist?) is (yes) not (no) +:getRightOppositeLanelet; +repeat while (Opposite direction Lanelet exist?) is (yes) not (no) +} +:compute\n(class ObjectData).to_road_shoulder_distance; +note right +distance from overhang_pose +to right most linestring +end note +endif +endif +} + +partition calcRawShiftPointsFromObjects() { +:compute max_allowable_lateral_distance; +note right +The sum of +- lat_collision_safety_buffer +- lat_collision_margin +- vehicle_width +end note +:compute max_shift_length; +note right +subtract +- road_shoulder_safety_margin +- 0.5 x vehicle_width +from (class ObjectData).to_road_shoulder_margin +end note +if(isOnRight(object)?) then (yes) +if((class ObjectData).to_road_shoulder_distance \n\l>\l\n max_allowable_lateral_distance ?) then (yes) +:max_left_shift_limit = max_shift_length; +else (\n No) +:max_left_shift_limit = 0.0; +endif +else (\n No) +if((class ObjectData).to_road_shoulder_distance \n\l>\l\n max_allowable_lateral_distance ?) then (yes) +:max_right_shift_limit = -max_shift_length; +else (\n No) +:max_right_shift_limit = 0.0; +endif +endif +:compute shift length; +} +stop +@enduml +``` + + + #### How to keep the consistency of the planning TODO @@ -250,7 +392,8 @@ TODO | :----------------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | | resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | | resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 3.0 | -| lateral_collision_margin | [m] | double | The lateral distance between ego and avoidance targets. | 2.0 | +| lateral_collision_margin | [m] | double | The lateral distance between ego and avoidance targets. | 1.5 | +| lateral_collision_safety_buffer | [m] | double | Creates an additional gap that will prevent the vehicle from getting to near to the obstacle | 0.5 | | longitudinal_collision_margin_min_distance | [m] | double | when complete avoidance motion, there is a distance margin with the object for longitudinal direction. | 0.0 | | longitudinal_collision_margin_time | [s] | double | when complete avoidance motion, there is a time margin with the object for longitudinal direction. | 0.0 | | prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 1.0 | @@ -262,6 +405,7 @@ TODO | min_sharp_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1). | 1.0 | | max_right_shift_length | [m] | double | Maximum shift length for right direction | 5.0 | | max_left_shift_length | [m] | double | Maximum shift length for left direction | 5.0 | +| road_shoulder_safety_margin | [m] | double | Prevents the generated path to come too close to the road shoulders. | 0.5 | ### Speed limit modification diff --git a/planning/behavior_path_planner/behavior_path_planner_design_limitations.md b/planning/behavior_path_planner/behavior_path_planner_design_limitations.md new file mode 100644 index 0000000000000..7ffd89f2b495c --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_design_limitations.md @@ -0,0 +1,52 @@ +# Design limitation + +The document describes the limitations that are currently present in the `behavior_path_planner` module. + +The following items (but not limited to) fall in the scope of limitation: + +- limitations due to the third-party API design and requirement +- limitations due to any shortcoming out of the developer's control. + +## Limitation: Multiple connected opposite lanes require Linestring with shared ID + +To fully utilize the `Lanelet2`'s [API](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletPrimitives.md#lanelet), the design of the vector map (`.osm`) needs to follow all the criteria described in `Lanelet2` documentation. Specifically, in the case of 2 or more lanes, the Linestrings that divide the current lane with the opposite/adjacent lane need to have a matching `Linestring ID`. Assume the following **ideal case**. + +![limitation01-01-ideal-case1](./image/limitations/limitation01-01.png) + +In the image, `Linestring ID51` is shared by `Lanelet A` and `Lanelet B`. Hence we can directly use the available `left`, `adjacentLeft`, `right`, `adjacentRight` and `findUsages` method within `Lanelet2`'s API to directly query the direction and opposite lane availability. + +```cpp +const auto right_lane = routing_graph_ptr_->right(lanelet); +const auto adjacent_right_lane = routing_graph_ptr_->adjacentRight(lanelet); +const auto opposite_right_lane = lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); +``` + +The following images show the situation where **these API does not work directly**. This means that we cannot use them straight away, and several assumptions and logical instruction are needed to make these APIs work. + +![limitation01-02-non-work](./image/limitations/limitation01-02-not-work.png) + +In this example (multiple linestring issues), `Lanelet C` contains `Linestring ID61` and `ID62`, while `Lanelet D` contains `Linestring ID63` and `ID 64`. Although the `Linestring ID62` and `ID64` have identical point IDs and seem visually connected, the API will treat these Linestring as though they are separated. When it searches for any `Lanelet` that is connected via `Linestring ID62`, it will return `NULL`, since `ID62` only connects to `Lanelet C` and not other `Lanelet`. + +Although, in this case, it is possible to forcefully search the lanelet availability by checking the lanelet that contains the points, using`getLaneletFromPoint` method. But, the implementation requires complex rules for it to work. Take the following images as an example. + +![limitation01-03-not-equal-length](./image/limitations/limitation01-03-not-equal-length.png) + +Assume `Object X` is in `Lanelet F`. We can forcefully search `Lanelet E` via `Point 7`, and it will work if `Point 7` is utilized by **only 2 lanelet**. However, the complexity increases when we want to start searching for the **direction** of the opposite lane. We can infer the direction of the lanelet by using mathematical operations (dot product of vector `V_ID72` (`Point 6` minus `Point 9`), and `V_ID74` (`Point 7` minus `Point 8`). But, notice that we did not use Point 7 in V_ID72. This is because searching it requires an iteration, adding additional non-beneficial computation. + +Suppose the points are used by **more than 2 lanelets**. In that case, we have to find the differences for all lanelet, and the result might be undefined. The reason is that the differences between the coordinates do not reflect the actual **shape** of the lanelet. The following image demonstrates this point. + +![equal-length-but-non-identical-shape](./image/limitations/limitation01-04-equal-length.png) + +![points-shared-more-than-one](./image/limitations/limitation01-04-not-equal.png) + +There are many other available solutions to try. However, further attempt to solve this might cause issues in the future, especially for maintaining or scaling up the software. + +In conclusion, the multiple Linestring issues will not be supported. Covering these scenarios might give the user an "everything is possible" impression. This is dangerous since any attempt to create a non-standardized vector map is not compliant with safety regulations. + +## Limitation: Avoidance at Corners and Intersections + +Currently, the implementation doesn't cover avoidance at corners and intersections. The reason is similar to here. However, this case can still be supported in the future (assuming the vector map is defined correctly). + +![limitation-at-the-intersections](./image/limitations/limitation-intersection.png) + +![limitation-at-the-corner](./image/limitations/limitation-corner.png) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index bfe310b73725f..34951cc91d30a 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -7,11 +7,15 @@ detection_area_right_expand_dist: 0.0 detection_area_left_expand_dist: 1.0 + enable_avoidance_over_same_direction: true + enable_avoidance_over_opposite_direction: true + threshold_distance_object_is_on_center: 1.0 # [m] threshold_speed_object_is_stopped: 1.0 # [m/s] object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] - lateral_collision_margin: 2.0 # [m] + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] @@ -20,6 +24,8 @@ min_nominal_avoidance_speed: 7.0 # [m/s] min_sharp_avoidance_speed: 1.0 # [m/s] + road_shoulder_safety_margin: 0.5 # [m] + max_right_shift_length: 5.0 max_left_shift_length: 5.0 @@ -30,7 +36,7 @@ # For prevention of large acceleration while avoidance min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/ss] + max_avoidance_acceleration: 0.5 # [m/s2] # for debug publish_debug_marker: false diff --git a/planning/behavior_path_planner/image/limitations/limitation-corner.png b/planning/behavior_path_planner/image/limitations/limitation-corner.png new file mode 100644 index 0000000000000..fe32071edd6dc Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation-corner.png differ diff --git a/planning/behavior_path_planner/image/limitations/limitation-intersection.png b/planning/behavior_path_planner/image/limitations/limitation-intersection.png new file mode 100644 index 0000000000000..eb78094b51655 Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation-intersection.png differ diff --git a/planning/behavior_path_planner/image/limitations/limitation01-01.png b/planning/behavior_path_planner/image/limitations/limitation01-01.png new file mode 100644 index 0000000000000..a84ec07ce940b Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-01.png differ diff --git a/planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png b/planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png new file mode 100644 index 0000000000000..c728f188bb387 Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png differ diff --git a/planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png b/planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png new file mode 100644 index 0000000000000..8d594e0ca5d94 Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png differ diff --git a/planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png b/planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png new file mode 100644 index 0000000000000..79daca2b4f502 Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png differ diff --git a/planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png b/planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png new file mode 100644 index 0000000000000..cd734fcd1e310 Binary files /dev/null and b/planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png differ diff --git a/planning/behavior_path_planner/image/obstacle_to_road_shoulder_distance.png b/planning/behavior_path_planner/image/obstacle_to_road_shoulder_distance.png new file mode 100644 index 0000000000000..724708d23ff80 Binary files /dev/null and b/planning/behavior_path_planner/image/obstacle_to_road_shoulder_distance.png differ diff --git a/planning/behavior_path_planner/image/shift_length_computation.png b/planning/behavior_path_planner/image/shift_length_computation.png new file mode 100644 index 0000000000000..5c6f364ececd0 Binary files /dev/null and b/planning/behavior_path_planner/image/shift_length_computation.png differ diff --git a/planning/behavior_path_planner/image/shift_length_parameters.png b/planning/behavior_path_planner/image/shift_length_parameters.png new file mode 100644 index 0000000000000..4bb16c03b0881 Binary files /dev/null and b/planning/behavior_path_planner/image/shift_length_parameters.png differ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index ea33116ea3965..3e3db6f912043 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -135,7 +135,7 @@ class AvoidanceModule : public SceneModuleInterface // -- path generation -- ShiftedPath generateAvoidancePath(PathShifter & shifter) const; - void generateExtendedDrivableArea(ShiftedPath * shifted_path, double margin) const; + void generateExtendedDrivableArea(ShiftedPath * shifted_path) const; // -- velocity planning -- void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 5c9ea12dbc429..3dee936f3b6a1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -52,6 +52,13 @@ struct AvoidanceParameters // lanelet expand length for left side to find avoidance target vehicles double detection_area_left_expand_dist = 1.0; + // enable avoidance to be perform only in lane with same direction + bool enable_avoidance_over_same_direction{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 enable_avoidance_over_opposite_direction{true}; + // Vehicles whose distance to the center of the path is // less than this will not be considered for avoidance. double threshold_distance_object_is_on_center; @@ -67,6 +74,9 @@ struct AvoidanceParameters // we want to keep this lateral margin when avoiding double lateral_collision_margin; + // a buffer in case lateral_collision_margin is set to 0. Will throw error + // don't ever set this value to 0 + double lateral_collision_safety_buffer{0.5}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction @@ -96,6 +106,10 @@ struct AvoidanceParameters // distance for avoidance. Need a sharp avoidance path to avoid the object. double min_sharp_avoidance_speed; + // The margin is configured so that the generated avoidance trajectory does not come near to the + // road shoulder. + double road_shoulder_safety_margin{1.0}; + // Even if the obstacle is very large, it will not avoid more than this length for right direction double max_right_shift_length; @@ -152,6 +166,15 @@ struct ObjectData // avoidance target // count up when object disappeared. Removed when it exceeds threshold. int lost_count = 0; + + // store the information of the lanelet which the object's overhang is currently occupying + lanelet::ConstLanelet overhang_lanelet; + + // the position of the overhang + Pose overhang_pose; + + // lateral distance from overhang to the road shoulder + double to_road_shoulder_distance{0.0}; }; using ObjectDataArray = std::vector; @@ -242,6 +265,7 @@ struct DebugData { std::shared_ptr expanded_lanelets; std::shared_ptr current_lanelets; + std::shared_ptr farthest_linestring_from_overhang; AvoidPointArray current_shift_points; // in path shifter AvoidPointArray new_shift_points; // in path shifter diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index 683677f3086ca..1181006d6fd61 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -46,7 +46,8 @@ void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & pa double calcDistanceToClosestFootprintPoint( const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos); -double calcOverhangDistance(const ObjectData & object_data, const Pose & base_pose); +double calcOverhangDistance( + const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose); void setEndData( AvoidPoint & ap, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index c732e810e4104..26abdd6978620 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -90,6 +90,12 @@ MarkerArray createPoseMarkerArray( const Pose & pose, const std::string & ns, const int64_t id, const double r, const double g, const double b); +MarkerArray makeOverhangToRoadShoulderMarkerArray( + const behavior_path_planner::ObjectDataArray & objects); + +MarkerArray createOvehangFurthestLineStringMarkerArray( + const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r, + const double g, const double b); } // namespace marker_utils std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index fa2b671893ce2..f49a932970473 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -54,6 +54,13 @@ namespace tier4_autoware_utils { +template <> +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose.position; +} + template <> inline geometry_msgs::msg::Pose getPose( const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3ed89165c8ba6..fcafa0144dd66 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -131,12 +131,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // Start timer. This must be done after all data (e.g. vehicle pose, velocity) are ready. { const auto planning_hz = declare_parameter("planning_hz", 10.0); - const auto period = rclcpp::Rate(planning_hz).period(); - auto on_timer = std::bind(&BehaviorPathPlannerNode::run, this); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(on_timer), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(planning_hz).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } } @@ -207,12 +204,15 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.resample_interval_for_output = dp("resample_interval_for_output", 3.0); p.detection_area_right_expand_dist = dp("detection_area_right_expand_dist", 0.0); p.detection_area_left_expand_dist = dp("detection_area_left_expand_dist", 1.0); + p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true); + p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true); p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0); p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0); p.object_check_forward_distance = dp("object_check_forward_distance", 150.0); p.object_check_backward_distance = dp("object_check_backward_distance", 2.0); p.lateral_collision_margin = dp("lateral_collision_margin", 2.0); + p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5); p.prepare_time = dp("prepare_time", 3.0); p.min_prepare_distance = dp("min_prepare_distance", 10.0); @@ -221,6 +221,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.min_nominal_avoidance_speed = dp("min_nominal_avoidance_speed", 5.0); p.min_sharp_avoidance_speed = dp("min_sharp_avoidance_speed", 1.0); + p.road_shoulder_safety_margin = dp("road_shoulder_safety_margin", 0.5); + p.max_right_shift_length = dp("max_right_shift_length", 1.5); p.max_left_shift_length = dp("max_left_shift_length", 1.5); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 7f7f5bb9120a2..5649c7e2a4025 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -156,6 +156,10 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( const lanelet::ConstLanelets & current_lanes, const PathWithLaneId & reference_path, DebugData & debug) const { + using lanelet::geometry::distance2d; + using lanelet::utils::getId; + using lanelet::utils::to2D; + const auto & path_points = reference_path.points; const auto & ego_pos = getEgoPosition(); @@ -183,6 +187,8 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( ? calcSignedArcLength(path_points, ego_pos, rh->getGoalPose().position) : std::numeric_limits::max(); + lanelet::ConstLineStrings3d debug_linestring; + debug_linestring.clear(); // for filtered objects ObjectDataArray target_objects; for (const auto & i : lane_filtered_objects_index) { @@ -221,18 +227,66 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( const auto object_closest_pose = path_points.at(object_closest_index).point.pose; object_data.lateral = calcLateralDeviation(object_closest_pose, object_pos); - // Object is on center line -> ignore. - if (std::abs(object_data.lateral) < parameters_.threshold_distance_object_is_on_center) { - DEBUG_PRINT("Ignore object: (object is on center line)"); + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_dist = + calcOverhangDistance(object_data, object_closest_pose, object_data.overhang_pose.position); + + lanelet::ConstLanelet overhang_lanelet; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) { continue; } - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = calcOverhangDistance(object_data, object_closest_pose); + if (overhang_lanelet.id()) { + object_data.overhang_lanelet = overhang_lanelet; + lanelet::BasicPoint3d overhang_basic_pose( + object_data.overhang_pose.position.x, object_data.overhang_pose.position.y, + object_data.overhang_pose.position.z); + if (isOnRight(object_data)) { + const auto & target_left_line = [this, &rh, &overhang_lanelet]() { + if ( + parameters_.enable_avoidance_over_same_direction && + parameters_.enable_avoidance_over_opposite_direction) { + return rh->getLeftMostLinestring(overhang_lanelet); + } else if ( + parameters_.enable_avoidance_over_same_direction && + !parameters_.enable_avoidance_over_opposite_direction) { + return rh->getLeftMostSameDirectionLinestring(overhang_lanelet); + } + return overhang_lanelet.leftBound(); + }(); + object_data.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(target_left_line.basicLineString())); + debug_linestring.push_back(target_left_line); + } else { + const auto & target_right_line = [this, &rh, &overhang_lanelet]() { + if ( + parameters_.enable_avoidance_over_same_direction && + parameters_.enable_avoidance_over_opposite_direction) { + return rh->getRightMostLinestring(overhang_lanelet); + } else if ( + parameters_.enable_avoidance_over_same_direction && + !parameters_.enable_avoidance_over_opposite_direction) { + return rh->getRightMostSameDirectionLinestring(overhang_lanelet); + } + return overhang_lanelet.rightBound(); + }(); + object_data.to_road_shoulder_distance = + distance2d(to2D(overhang_basic_pose), to2D(target_right_line.basicLineString())); + debug_linestring.push_back(target_right_line); + } + } DEBUG_PRINT( - "set object_data: longitudinal = %f, lateral = %f, largest_overhang = %f", - object_data.longitudinal, object_data.lateral, object_data.overhang_dist); + "set object_data: longitudinal = %f, lateral = %f, largest_overhang = %f," + "to_road_shoulder_distance = %f", + object_data.longitudinal, object_data.lateral, object_data.overhang_dist, + object_data.to_road_shoulder_distance); + + // Object is on center line -> ignore. + if (std::abs(object_data.lateral) < parameters_.threshold_distance_object_is_on_center) { + DEBUG_PRINT("Ignore object: (object is on center line)"); + continue; + } // set data target_objects.push_back(object_data); @@ -240,6 +294,8 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( // debug { + debug.farthest_linestring_from_overhang = + std::make_shared(debug_linestring); debug.current_lanelets = std::make_shared(current_lanes); debug.expanded_lanelets = std::make_shared(expanded_lanelets); } @@ -393,20 +449,42 @@ void AvoidanceModule::registerRawShiftPoints(const AvoidPointArray & future) AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( const ObjectDataArray & objects) const { - const auto avoid_margin = - parameters_.lateral_collision_margin + 0.5 * planner_data_->parameters.vehicle_width; const auto prepare_distance = getNominalPrepareDistance(); // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = getCurrentShift(); + // // implement lane detection here. + const auto & lat_collision_safety_buffer = parameters_.lateral_collision_safety_buffer; + const auto & lat_collision_margin = parameters_.lateral_collision_margin; + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto & road_shoulder_safety_margin = parameters_.road_shoulder_safety_margin; + const auto max_allowable_lateral_distance = + lat_collision_safety_buffer + lat_collision_margin + vehicle_width; + + const auto avoid_margin = + lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width; AvoidPointArray avoid_points; for (auto & o : objects) { - // calc shift length with margin and shift limit - const auto shift_length = isOnRight(o) - ? std::min(o.overhang_dist + avoid_margin, getLeftShiftBound()) - : std::max(o.overhang_dist - avoid_margin, getRightShiftBound()); + const auto max_shift_length = + o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width; + const auto max_left_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length, + this]() noexcept { + const auto left_shift_constraint = std::min(getLeftShiftBound(), max_shift_length); + return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? left_shift_constraint + : 0.0; + }; + + const auto max_right_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length, + this]() noexcept { + const auto right_shift_constraint = std::max(getRightShiftBound(), -max_shift_length); + return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? right_shift_constraint + : 0.0; + }; + const auto shift_length = isOnRight(o) + ? std::min(o.overhang_dist + avoid_margin, max_left_shift_limit()) + : std::max(o.overhang_dist - avoid_margin, max_right_shift_limit()); const auto avoiding_shift = shift_length - current_ego_shift; const auto return_shift = shift_length; @@ -1584,22 +1662,211 @@ double AvoidanceModule::getLeftShiftBound() const return parameters_.max_left_shift_length; } -void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path, double margin) const +// TODO(murooka) judge when and which way to extend drivable area. current implementation is keep +// extending during avoidance module +// TODO(murooka) freespace during turning in intersection where there is no neighbour lanes +// NOTE: Assume that there is no situation where there is an object in the middle lane of more than +// two lanes since which way to avoid is not obvious +void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) const { - const auto right_extend_elem = - std::min_element(shifted_path->shift_length.begin(), shifted_path->shift_length.end()); - const auto left_extend_elem = - std::max_element(shifted_path->shift_length.begin(), shifted_path->shift_length.end()); + const auto & route_handler = planner_data_->route_handler; + lanelet::ConstLanelets extended_lanelets = avoidance_data_.current_lanelets; + + { + // 0. Extend to right/left of objects + for (const auto & obstacle : avoidance_data_.objects) { + auto object_lanelet = obstacle.overhang_lanelet; + if (isOnRight(obstacle)) { + auto lanelet_at_left = route_handler->getLeftLanelet(object_lanelet); + while (lanelet_at_left) { + extended_lanelets.push_back(lanelet_at_left.get()); + lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); + } + auto lanelet_at_right = + planner_data_->route_handler->getRightLanelet(lanelet_at_left.get()); + while (lanelet_at_right) { + extended_lanelets.push_back(lanelet_at_right.get()); + lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); + } + } else { + auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet); + while (lanelet_at_right) { + extended_lanelets.push_back(lanelet_at_right.get()); + lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); + } + auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); + while (lanelet_at_left) { + extended_lanelets.push_back(lanelet_at_left.get()); + lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); + } + } + } + } + + for (const auto & lane : avoidance_data_.current_lanelets) { + { // 1. extend to right/left or adjacent right/left (where lane_change tag = no, but not a + // problem to extend for avoidance) lane if it exists + // this can be available only if line string is shared + const auto opt_right_lane = route_handler->getRightLanelet(lane); + const auto opt_left_lane = route_handler->getLeftLanelet(lane); + + if (opt_right_lane) { + extended_lanelets.push_back(opt_right_lane.get()); + continue; + } else if (opt_left_lane) { + extended_lanelets.push_back(opt_left_lane.get()); + continue; + } + } + + { // 2. when there are multiple turning lanes whose previous lanelet is the same in + // intersection + const bool update_extended_lanelets = [&]() { + // lanelet is not turning lane + const std::string turn_direction = lane.attributeOr("turn_direction", "none"); + if (turn_direction != "right" && turn_direction != "left") { + return false; + } + + // get previous lane, and return false if previous lane does not exist + lanelet::ConstLanelet prev_lane; + if (!route_handler->getPreviousLaneletWithinRoute(lane, &prev_lane)) { + return false; + } - double right_extend = std::min(*right_extend_elem, 0.0); - double left_extend = std::max(*left_extend_elem, 0.0); + // get next lanes from the previous lane, and return false if next lanes do not exist + const auto next_lanes = route_handler->getNextLanelets(lane); + if (next_lanes.empty()) { + return false; + } + + // look for neighbour lane, where end line of the lane is connected to end line of the + // original lane + for (const auto & next_lane : next_lanes) { + if (lane.id() == next_lane.id()) { + continue; + } + + const Eigen::Vector2d & next_left_back_point_2d = + next_lane.leftBound2d().back().basicPoint(); + const Eigen::Vector2d & next_right_back_point_2d = + next_lane.rightBound2d().back().basicPoint(); + + const Eigen::Vector2d & orig_left_back_point_2d = lane.leftBound2d().back().basicPoint(); + const Eigen::Vector2d & orig_right_back_point_2d = + lane.rightBound2d().back().basicPoint(); + + constexpr double epsilon = 1e-5; + const bool is_neighbour_lane = + (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon || + (next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon; + if (is_neighbour_lane) { + extended_lanelets.push_back(next_lane); + return true; + } + } + + return false; + }(); + if (update_extended_lanelets) { + continue; + } + } - constexpr double THRESHOLD = 0.01; - right_extend -= (right_extend < -THRESHOLD) ? margin : 0.0; - left_extend += (left_extend > THRESHOLD) ? margin : 0.0; + { // 3. deal with the problem that line string is not shared to neighbour lanelets in + // intersection (for left lane), assuming that points are shared + // this part will be removed when the map format is modified correctly wrt sharing line string + // since 1 works for this + bool update_extended_lanelets = false; + const auto & left_lane_candidates = + route_handler->getLaneletsFromPoint(lane.leftBound().front()); + for (const auto & left_lane_candidate : left_lane_candidates) { + const Eigen::Vector2d & left_lane_right_back_point_2d = + left_lane_candidate.rightBound2d().back().basicPoint(); + const Eigen::Vector2d & orig_lane_left_back_point_2d = + lane.leftBound2d().back().basicPoint(); + + const double epsilon = 1e-5; + const bool is_neighbour_lane = + (left_lane_right_back_point_2d - orig_lane_left_back_point_2d).norm() < epsilon; + if (is_neighbour_lane) { + extended_lanelets.push_back(left_lane_candidate); + update_extended_lanelets = true; + break; + } + } + if (update_extended_lanelets) { + continue; + } + } - const auto extended_lanelets = lanelet::utils::getExpandedLanelets( - avoidance_data_.current_lanelets, left_extend, right_extend); + { // 4. deal with the problem that line string is not shared to neighbour lanelets in + // intersection (for right lane), assuming that points are shared + // this part will be removed if the map format is modified correctly wrt sharing line string + // since 1 works for this + bool update_extended_lanelets = false; + const auto & right_lane_candidates = + route_handler->getLaneletsFromPoint(lane.rightBound().front()); + for (const auto & right_lane_candidate : right_lane_candidates) { + const Eigen::Vector2d & right_lane_left_back_point_2d = + right_lane_candidate.leftBound2d().back().basicPoint(); + const Eigen::Vector2d & orig_lane_right_back_point_2d = + lane.rightBound2d().back().basicPoint(); + + const double epsilon = 1e-5; + const bool is_neighbour_lane = + (right_lane_left_back_point_2d - orig_lane_right_back_point_2d).norm() < epsilon; + if (is_neighbour_lane) { + extended_lanelets.push_back(right_lane_candidate); + update_extended_lanelets = true; + break; + } + } + if (update_extended_lanelets) { + continue; + } + } + + { + // 5. if drivable area cannot be extended inside the same-direction lane, extend to even + // opposite lane + const auto opposite_lanes = route_handler->getRightOppositeLanelets(lane); + + if (!opposite_lanes.empty()) { + for (const auto & opposite_lane : opposite_lanes) { + extended_lanelets.push_back(opposite_lane); + } + continue; + } + } + + { // 6. deal with the problem that line string is not shared to neighbour opposite lanelet, + // assuming that points are shared + // this part will be removed when the map format is modified correctly wrt sharing line string + // since 5 works for this + bool update_extended_lanelets = false; + const auto & opposite_lane_candidates = + route_handler->getLaneletsFromPoint(lane.rightBound().front()); + for (const auto & opposite_lane_candidate : opposite_lane_candidates) { + const Eigen::Vector2d & opposite_lane_right_front_point_2d = + opposite_lane_candidate.rightBound2d().front().basicPoint(); + const Eigen::Vector2d & orig_lane_right_back_point_2d = + lane.rightBound2d().back().basicPoint(); + + const double epsilon = 1e-5; + const bool is_neighbour_lane = + (opposite_lane_right_front_point_2d - orig_lane_right_back_point_2d).norm() < epsilon; + if (is_neighbour_lane) { + extended_lanelets.push_back(opposite_lane_candidate); + update_extended_lanelets = true; + break; + } + } + if (update_extended_lanelets) { + continue; + } + } + } { const auto & p = planner_data_->parameters; @@ -1821,8 +2088,7 @@ BehaviorModuleOutput AvoidanceModule::plan() debug_data_.output_shift = avoidance_path.shift_length; // Drivable area generation. - constexpr double extend_margin = 0.5; - generateExtendedDrivableArea(&avoidance_path, extend_margin); + generateExtendedDrivableArea(&avoidance_path); // modify max speed to prevent acceleration in avoidance maneuver. modifyPathVelocityToPreventAccelerationOnAvoidance(avoidance_path); @@ -2343,10 +2609,12 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData using marker_utils::createAvoidPointMarkerArray; using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; + using marker_utils::createOvehangFurthestLineStringMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftPointMarkerArray; + using marker_utils::makeOverhangToRoadShoulderMarkerArray; debug_marker_.markers.clear(); @@ -2373,6 +2641,9 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); add(createAvoidanceObjectsMarkerArray(avoidance_data_.objects, "avoidance_object")); + add(makeOverhangToRoadShoulderMarkerArray(avoidance_data_.objects)); + add(createOvehangFurthestLineStringMarkerArray( + *debug.farthest_linestring_from_overhang, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); // parent object info addAvoidPoint(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 623f0c97200b1..e9865b069eeca 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -141,7 +141,8 @@ double calcDistanceToClosestFootprintPoint( return distance; } -double calcOverhangDistance(const ObjectData & object_data, const Pose & base_pose) +double calcOverhangDistance( + const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose) { double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number @@ -151,8 +152,22 @@ double calcOverhangDistance(const ObjectData & object_data, const Pose & base_po for (const auto & p : object_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point); - largest_overhang = isOnRight(object_data) ? std::max(largest_overhang, lateral) - : std::min(largest_overhang, lateral); + + const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { + if (lateral > largest_overhang) { + overhang_pose = point; + } + return std::max(largest_overhang, lateral); + }; + + const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() { + if (lateral < largest_overhang) { + overhang_pose = point; + } + return std::min(largest_overhang, lateral); + }; + + largest_overhang = isOnRight(object_data) ? overhang_pose_on_right() : overhang_pose_on_left(); } return largest_overhang; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index e3b9eed8859ad..941e4b09fa0a3 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -481,7 +481,82 @@ MarkerArray createPoseMarkerArray( return msg; } +MarkerArray makeOverhangToRoadShoulderMarkerArray( + const behavior_path_planner::ObjectDataArray & objects) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker{}; + marker.header.frame_id = "map"; + marker.header.stamp = current_time; + marker.ns = "overhang"; + + const auto normal_color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 1.0); + + int32_t i = 0; + for (const auto & object : objects) { + marker.id = i++; + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.type = Marker::TEXT_VIEW_FACING; + // marker.action = Marker::ADD; + marker.pose = object.overhang_pose; + marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0); + marker.color = normal_color; + std::ostringstream string_stream; + string_stream << "(to_road_shoulder_distance = " << object.to_road_shoulder_distance << " [m])"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createOvehangFurthestLineStringMarkerArray( + const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r, + const double g, const double b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + for (const auto & linestring : linestrings) { + Marker marker{}; + marker.header.frame_id = "map"; + marker.header.stamp = current_time; + + marker.ns = ns; + marker.id = linestring.id(); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.type = Marker::LINE_STRIP; + marker.action = Marker::ADD; + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = tier4_autoware_utils::createMarkerScale(0.4, 0.0, 0.0); + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.999); + for (const auto & p : linestring.basicLineString()) { + Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + marker.points.push_back(point); + } + msg.markers.push_back(marker); + marker.ns = "linestring id"; + marker.type = Marker::TEXT_VIEW_FACING; + Pose text_id_pose; + marker.scale = tier4_autoware_utils::createMarkerScale(1.5, 1.5, 1.5); + marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.8); + text_id_pose.position.x = linestring.front().x(); + text_id_pose.position.y = linestring.front().y(); + text_id_pose.position.z = linestring.front().z(); + marker.pose = text_id_pose; + std::ostringstream ss; + ss << "(ID : " << linestring.id() << ") "; + marker.text = ss.str(); + msg.markers.push_back(marker); + } + + return msg; +} } // namespace marker_utils std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr) diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index a7ea3675ba8f4..19c3a1d72264c 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -13,7 +13,7 @@ It consists of several modules. Please refer to the links listed below for detai - [Stop Line](stop-line-design.md) - [Traffic Light](traffic-light-design.md) - [Occlusion Spot](occlusion-spot-design.md) -- [No Stopping Area](no-stopping-area.design.md) +- [No Stopping Area](no-stopping-area-design.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/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index b45bd4e165a98..0ae1c7b705c03 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -2,24 +2,24 @@ ros__parameters: occlusion_spot: pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity - safety_time_buffer: 0.1 # [m/s] assume pedestrian is dashing from occlusion at this velocity + safety_time_buffer: 0.1 # [s] delay for the system response threshold: - detection_area_length: 200.0 # [m] the length of path to consider occlusion spot + detection_area_length: 100.0 # [m] the length of path to consider perception range stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision # road type parameters public_road: - min_velocity: 2.78 # [m/s] minimum velocity to ignore occlusion spot - ebs_decel: -6.0 # [m/s^2] maximum deceleration to assume for emergency stops. + min_velocity: 1.5 # [m/s] minimum velocity to ignore occlusion spot + ebs_decel: -2.5 # [m/s^2] maximum deceleration to assume for emergency stops. pbs_decel: -1.0 # [m/s^2] deceleration to assume for predictive braking stops private_road: min_velocity: 1.5 # [m/s] minimum velocity to ignore occlusion spot - ebs_decel: -4.0 # [m/s^2] maximum deceleration to assume for emergency stops. - pbs_decel: -0.5 # [m/s^2] deceleration to assume for predictive braking stops. + ebs_decel: -2.0 # [m/s^2] maximum deceleration to assume for emergency stops. + pbs_decel: -1.0 # [m/s^2] deceleration to assume for predictive braking stops. sidewalk: min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. - slice_size: 0.5 # [m] size of sidewalk slices in both length and distance relative to the ego trajectory. - focus_range: 2.5 # [m] buffer around the ego path used to build the sidewalk area. + slice_size: 1.5 # [m] size of sidewalk slices in both length and distance relative to the ego trajectory. + focus_range: 3.0 # [m] buffer around the ego path used to build the sidewalk area. grid: free_space_max: 40 # maximum value of a free space cell in the occupancy grid occupied_min: 60 # minimum value of an occupied cell in the occupancy grid diff --git a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml index 9df48644f74f0..267dde50c7970 100644 --- a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml +++ b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml @@ -4,4 +4,5 @@ max_delay_sec: 3.0 near_line_distance: 1.0 dead_line_margin: 1.0 + max_yaw_deviation_deg: 90.0 check_timeout_after_stop_line: true diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg index ffa95eff81d9a..83c803e828e67 100644 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg @@ -1,326 +1,4 @@ + - - - - - - - - -
-
-
v [m/s]
-
-
- - v [m/s] - - - - - - - - -
-
-
s [m]
-
-
- - s [m] - - - - - - - - - - -
-
-
x
-
-
- - x - - - - - - -
-
-
x
-
-
- - x - - - - - - -
-
-
- min velocity -
-
-
- - min velocity - - - - - - -
-
-
- PBS maximum allowed deceleration -
-
-
- - PBS maximum allowed deceleration - - - - - -
-
-
- - - - -  reference maximum velocity
-
- - --- - occlusion spot plan velocity - -
- - - -      safe velocity consider EBS at collision point
-      current velocity
-PBS : predictive braking system to slow down before reaching collision point
-EBS : emergency braking system to stop if obstacle rush out
- - -
-
-
- - - -  reference maximum velocity... - - - - - - -
-
-
x
-
-
- - x - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
Goal
-
-
- - Goal - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
x
-
-
- - x - - - - - - - -Viewer does not support full SVG 1.1 - - - +
v [m/s]
v [m/s]
s [m]
s [m]
x
x
min velocity
min velocity
- -  reference maximum velocity
---  occlusion spot plan velocity
     PBS safe velocity consider EBS at collision point
     current velocity
PBS : predictive braking system to slow down before reaching collision point
EBS : emergency braking system to stop if obstacle rush out
- -  reference maximum velocity...
x
x
x
x
x
x
Goal
Goal
maximum allowed deceleration
maximum allowed decelerationText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 2e57dc7f40236..d5acadfcf2311 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -20,8 +20,13 @@ #include #include #include +#include +#include +#include +#include #include +#include #include #include @@ -32,11 +37,30 @@ #include #include +#include #include namespace behavior_velocity_planner { +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using lanelet::ArcCoordinates; +using lanelet::BasicLineString2d; +using lanelet::BasicPoint2d; +using lanelet::BasicPolygon2d; +using lanelet::ConstLineString2d; +using lanelet::LaneletMapPtr; +using lanelet::geometry::fromArcCoordinates; +using lanelet::geometry::toArcCoordinates; +using DetectionAreaIdx = boost::optional>; + namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; @@ -118,14 +142,69 @@ struct PossibleCollisionInfo } }; +lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); + +// Note : consider offset_from_start_to_ego and safety margin for collision here +inline void handleCollisionOffset( + std::vector & possible_collisions, double offset, double margin) +{ + for (auto & pc : possible_collisions) { + pc.arc_lane_dist_at_collision.length -= offset; + pc.arc_lane_dist_at_collision.length -= margin; + } +} + +inline double offsetFromStartToEgo( + const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx) +{ + double offset_from_ego_to_closest = 0; + for (int i = 0; i < closest_idx; i++) { + const auto & curr_p = path.points.at(i).point.pose.position; + const auto & next_p = path.points.at(i + 1).point.pose.position; + offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p); + } + const double offset_from_closest_to_target = + -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose) + .position.x; + return offset_from_ego_to_closest + offset_from_closest_to_target; +} + +inline void clipPathByLength( + const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0) +{ + double length_sum = 0; + for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { + length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + if (length_sum > max_length) return; + clipped.points.emplace_back(path.points.at(i)); + } +} + +inline bool isStuckVehicle(PredictedObject obj, const double min_vel) +{ + if ( + obj.classification.at(0).label == ObjectClassification::CAR || + obj.classification.at(0).label == ObjectClassification::TRUCK || + obj.classification.at(0).label == ObjectClassification::BUS) { + if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { + return true; + } + } + return false; +} +void filterCollisionByRoadType( + std::vector & possible_collisions, const DetectionAreaIdx road_type); bool splineInterpolate( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger); + const PathWithLaneId & input, const double interval, PathWithLaneId * output, + const rclcpp::Logger logger); +std::vector getParkedVehicles( + const PredictedObjects & dyn_objects, const PlannerParam & param, + std::vector & debug_point); +std::vector generatePossibleCollisionBehindParkedVehicle( + const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, + const std::vector & dyn_objects); ROAD_TYPE getCurrentRoadType( - const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr); -//!< @brief build a Lanelet from a interpolated path -lanelet::ConstLanelet buildPathLanelet( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const lanelet::ConstLanelet & current_lanelet, const LaneletMapPtr & lanelet_map_ptr); //!< @brief calculate intersection and collision point from occlusion spot void calculateCollisionPathPointFromOcclusionSpot( PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point, @@ -133,38 +212,28 @@ void calculateCollisionPathPointFromOcclusionSpot( const PlannerParam & param); //!< @brief create hidden collision behind parked car void createPossibleCollisionBehindParkedVehicle( - std::vector & possible_collisions, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PlannerParam & param, - const double offset_from_ego_to_target, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & dyn_obj_arr); + std::vector & possible_collisions, const PathWithLaneId & path, + const PlannerParam & param, const double offset_from_ego_to_target, + const PredictedObjects::ConstSharedPtr & dyn_obj_arr); //!< @brief set velocity and orientation to collision point based on previous Path with laneId void calcSlowDownPointsForPossibleCollision( - const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double offset_from_ego_to_target, std::vector & possible_collisions); + const int closest_idx, const PathWithLaneId & path, const double offset, + std::vector & possible_collisions); //!< @brief extract lanelet that includes target_road_type only -bool extractTargetRoad( - const int closest_idx, const lanelet::LaneletMapPtr lanelet_map_ptr, const double max_range, - const autoware_auto_planning_msgs::msg::PathWithLaneId & src_path, - double & offset_from_closest_to_target, - autoware_auto_planning_msgs::msg::PathWithLaneId & tar_path, const ROAD_TYPE & target_road_type); -//!< @brief generate collision coming from occlusion spots of the given grid map and lanelet map -void generatePossibleCollisions( - std::vector & possible_collisions, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const grid_map::GridMap & grid, - const double offset_from_ego_to_closest, const double offset_from_closest_to_target, - const PlannerParam & param, std::vector & debug); +DetectionAreaIdx extractTargetRoadArcLength( + const LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path, + const ROAD_TYPE & target_road_type); //!< @brief convert a set of occlusion spots found on sidewalk slice void generateSidewalkPossibleCollisionFromOcclusionSpot( std::vector & possible_collisions, const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, - const double offset_form_ego_to_target, const lanelet::ConstLanelet & path_lanelet, + const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param); //!< @brief generate possible collisions coming from occlusion spots on the side of the path void generateSidewalkPossibleCollisions( std::vector & possible_collisions, const grid_map::GridMap & grid, - const double offset_from_ego_to_closest, const double offset_from_closest_to_target, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, - std::vector & debug); + const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, + std::vector & debug); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp index 586678f8cd2f0..93d382576c29e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp @@ -48,6 +48,8 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface std::string road_type = "private"; std::vector sidewalks; std::vector possible_collisions; + PathWithLaneId path_raw; + PathWithLaneId interp_path; }; OcclusionSpotInPrivateModule( diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp index f9b93d37ed6b9..250972dea8342 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -46,6 +46,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface std::string road_type = "public"; double z; std::vector sidewalks; + std::vector parked_vehicle_point; std::vector possible_collisions; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 457a15eb3251b..8c0fecac9dbfd 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -68,6 +68,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface double max_delay_sec; double near_line_distance; double dead_line_margin; + double max_yaw_deviation_rad; bool check_timeout_after_stop_line; }; diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 3a244ae952814..4926e53542ba0 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -16,6 +16,7 @@ #define UTILIZATION__UTIL_HPP_ #include +#include #include #include @@ -41,6 +42,16 @@ #include #include +namespace tier4_autoware_utils +{ +template <> +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose.position; +} +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { using Point2d = boost::geometry::model::d2::point_xy; diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index cedb85154aef3..b936ac55a7d31 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -91,8 +91,20 @@ Note that the accuracy and performance of this search method is limited due to t title modifyPathVelocity (Private/Public) Road start -:get current road type; +partition process_path { +:clip path by length; +:interpolate path; +note right + using spline interpolation and interpolate (x,y,z,v) +end note +:calc closest path point from ego; +:extract target road pair; +note right +extract target road type start and end pair and early return if none +end note +} +partition process_sensor_data { if (road type is PUBLIC) then (yes) :preprocess dynamic object; else if (road type is PRIVATE) then (yes) @@ -100,22 +112,41 @@ else if (road type is PRIVATE) then (yes) else (no) stop endif - +} +partition find_possible_collision { +:calculate offset from start to ego; :generate possible collision; +:calculate collision path point and intersection point; +note right + - occlusion spot is calculated by longitudinally closest point of unknown cells. + - intersection point is where ego front bumper and darting object will crash. + - collision path point is calculated by arc coordinate consider ego vehicle's geometry. +end note -:find possible collision between path and occlusion spot; - -if (possible collision is found?) then (yes) -else (no) - stop -endif - -:calculate collision path point; - -:calculate safe velocity consider lateral distance and safe velocity; - -:insertSafeVelocityToPath; - +} +partition process_possible_collision { +:filter possible collision by road type; +note right +filter by target road type start and end pair +end note +:calculate slow down points for possible collision; +note right +calculate original velocity and height for the possible collision +end note +:handle collision offset; +note right +consider offset from path start to ego vehicle for possible collision +end note +:apply safe velocity consider possible collision; +note right +calculated by +- ebs deceleration [m/s] emergency braking system consider lateral distance to the occlusion spot. +- maximum allowed deceleration [m/s^2] +- min velocity [m/s] the velocity that is allowed on the road. +- original_velocity [m/s] +set minimum velocity for path point after occlusion spot. +end note +} stop @enduml ``` @@ -126,36 +157,28 @@ stop @startuml title modifyPathVelocity For Public Road start -partition parking_vehicle_selection { -:get all lanelet id on ego path; -:get right/left lanelets around ego path; -:get center lane lines from lanelets around ego path; -:get parked vehicle from dynamic object array; + +partition process_path { +:clip path by length; note right - target parked vehicle is define as follow . - - dynamic object's semantic type is "car","bus","track". - - velocity is below `stuck_vehicle_vel`. - - lateral position from lane center is farther than `lateral_deviation_threshold`. + 100m considering perception range end note -} -partition offset_calculation { :interpolate ego path; -note right - using spline interpolation and interpolate (x,y,z,v) -end note :get closest index from ego position in interpolated path; -:calculate offset from path start to ego; +:extract target road type start/end distance by arc length; } -:convert interpolated path to `path_lanelet`; +partition preprocess_dynamic_object { +:get parked vehicle from dynamic object array; note right - `path_lanelet` is lanelet which is created by ego path - and mainly used for arc coordinate conversion. + target parked vehicle is define as follow . + - dynamic object's semantic type is "car","bus","track". + - velocity is below `stuck_vehicle_vel`. end note -partition generate_possible_collision { +} +partition find_possible_collision { :generate possible collision behind parked vehicle; note right - occlusion spot candidate is stuck vehicle polygon 2 points farther which is closer to ego path. - - consider occlusion which is nearer than `lateral_distance_threshold`. end note :calculate collision path point and intersection point; note right @@ -164,25 +187,13 @@ note right - collision path point is calculated by arc coordinate consider ego vehicle's geometry. end note } -:extract target road type start/end distance by arc length; -:calculate original velocity and height for the possible collision; -note right -calculated (x,y,z,v) at the path using linear interpolation between interpolated path points. -end note -partition calculate_safe_velocity { +partition process_possible_collision { +:filter collision by road type; +:calculate slow down points for possible collision; +:handle collision offset; :calculate safe velocity consider lateral distance and safe velocity; -note right -calculated by -- ebs deceleration [m/s] emergency braking system consider lateral distance to the occlusion spot. -- pbs deceleration [m/s] predictive braking system consider distance to the possible collision. -- min velocity [m/s] the velocity that is allowed on the road. -- original_velocity [m/s] -end note -} :insert safe velocity to path; -note right - set minimum velocity for path point after occlusion spot. -end note +} stop @enduml ``` @@ -193,6 +204,15 @@ stop @startuml title modifyPathVelocity For Private Road start + +partition process_path { +:clip path by length; +note right + 50m considering occupancy grid range +end note +:interpolate ego path; +:get closest index from ego position in interpolated path; +} partition occupancy_grid_preprocess { :convert occupancy grid to image; note right @@ -208,51 +228,22 @@ note right convert from occupancy grid to image to use opencv functions. end note } -partition offset_calculation { -:interpolate ego path; -note right - using spline interpolation and interpolate (x,y,z,v) -end note -:get closest index from ego position in interpolated path; -:calculate offset from path start to ego; -} -:convert interpolated path to `path_lanelet`; -note right - `path_lanelet` is lanelet which is created by ego path - and mainly used for arc coordinate conversion. -end note partition generate_possible_collision { +:calculate offset from path start to ego; :generate possible collision from occlusion spot; note right - occlusion spot candidate is N by N size unknown cells. - consider occlusion which is nearer than `lateral_distance_threshold`. end note :calculate collision path point and intersection point; -note right - - occlusion spot is calculated by longitudinally closest point of unknown cells. - - intersection point is where ego front bumper and darting object will crash. - - collision path point is calculated by arc coordinate consider ego vehicle's geometry. -end note } -:extract target road type start/end distance by arc length; -:calculate original velocity and height for the possible collision; -note right -calculated (x,y,z,v) at the path using linear interpolation between interpolated path points. -end note -partition calculate_safe_velocity { +partition handle_possible_collision { +:filter collision by road type; +:calculate slow down points for possible collision; +:handle collision offset; :calculate safe velocity consider lateral distance and safe velocity; -note right -calculated by -- ebs deceleration [m/s] emergency braking system consider lateral distance to the occlusion spot. -- pbs deceleration [m/s] predictive braking system consider distance to the possible collision. -- min velocity [m/s] the velocity that is allowed on the road. -- original_velocity [m/s] -end note -} :insert safe velocity to path; -note right - set minimum velocity for path point after occlusion spot. -end note +} stop @enduml ``` diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp index df4ff14dc8e91..e0f7745ca0407 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp @@ -212,7 +212,7 @@ visualization_msgs::msg::MarkerArray createObstacleMarkerArray( auto marker = createDefaultMarker( "map", now, "obstacles", 0, visualization_msgs::msg::Marker::SPHERE, createMarkerColor(1.0, 0.0, 0.0, 0.999)); - marker.scale = createMarkerScale(0.3, 0.3, 0.3); + marker.scale = createMarkerScale(0.6, 0.6, 0.6); marker.lifetime = rclcpp::Duration::from_seconds(0.5); for (size_t i = 0; i < obstacle_points.size(); ++i) { diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 2d1159fc1a1da..c8f6c034a8c37 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -171,11 +171,13 @@ boost::optional findForwardOffsetSegment( const auto p_front = to_bg2d(path.points.at(i).point.pose.position); const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position); - sum_length += bg::distance(p_front, p_back); + const auto segment_length = bg::distance(p_front, p_back); + sum_length += segment_length; // If it's over offset point, return front index and remain offset length if (sum_length >= offset_length) { - return std::make_pair(i, sum_length - offset_length); + const auto remain_length = sum_length - offset_length; + return std::make_pair(i, segment_length - remain_length); } } @@ -391,6 +393,8 @@ std::vector DetectionAreaModule::getObstaclePoints() for (const auto p : points) { if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(detection_area).basicPolygon())) { obstacle_points.push_back(planning_utils::toRosPoint(p)); + // get all obstacle point becomes high computation cost so skip if any point is found + break; } } } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index adda9e2189ae9..57023f4f26555 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -129,9 +129,9 @@ bool splineInterpolate( output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); } if (output->points.size() > 1) { - size_t l = resampled_s.size(); + size_t l = output->points.size(); output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; - output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation; + output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation; } return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 53a70b3af8c26..2511b2eb8ac43 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -102,9 +102,9 @@ bool splineInterpolate( output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); } if (output->points.size() > 1) { - size_t l = resampled_s.size(); + size_t l = output->points.size(); output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; - output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation; + output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation; } return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index ad7a7f7ffd784..9dea5b7784ec3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -129,29 +129,63 @@ std::vector makeCollisionMarkers( return debug_markers; } -std::vector makePolygonMarker( - const lanelet::BasicPolygon2d & polygon, double z, int id) +visualization_msgs::msg::MarkerArray makePolygonMarker( + const std::vector & polygons, double z) { - std::vector debug_markers; + visualization_msgs::msg::MarkerArray debug_markers; visualization_msgs::msg::Marker debug_marker; debug_marker.header.frame_id = "map"; - debug_marker.id = id; + debug_marker.id = 0; debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; debug_marker.action = visualization_msgs::msg::Marker::ADD; debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z); debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.05, 0.05, 0.05); + debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); debug_marker.ns = "sidewalk"; - for (const auto & p : polygon) { - geometry_msgs::msg::Point point = tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); - debug_marker.points.push_back(point); + for (const auto & poly : polygons) { + for (const auto & p : poly) { + geometry_msgs::msg::Point point = + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); + debug_marker.points.push_back(point); + } + debug_markers.markers.push_back(debug_marker); + debug_marker.id++; + debug_marker.points.clear(); } - debug_markers.push_back(debug_marker); return debug_markers; } +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const double r, + const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + int32_t uid = planning_utils::bitShift(lane_id); + int32_t i = 0; + for (const auto & p : path.points) { + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = ns; + marker.id = uid + i++; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = p.point.pose; + marker.scale = createMarkerScale(0.6, 0.3, 0.3); + if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { + // if p.lane_ids has lane_id + marker.color = createMarkerColor(r, g, b, 0.999); + } else { + marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); + } + msg.markers.push_back(marker); + } + + return msg; +} + template visualization_msgs::msg::MarkerArray createMarkers( T & debug_data, [[maybe_unused]] const int64_t module_id_) @@ -170,19 +204,12 @@ visualization_msgs::msg::MarkerArray createMarkers( // draw virtual wall markers int id = 0; - const double min_dist_between_markers = 5.0; - double prev_length = 0; for (const auto & possible_collision : possible_collisions) { - if ( - possible_collision.arc_lane_dist_at_collision.length - prev_length > - min_dist_between_markers) { - prev_length = possible_collision.arc_lane_dist_at_collision.length; - std::vector collision_markers = - makeSlowDownMarkers(possible_collision, debug_data.road_type, id++); - occlusion_spot_slowdown_markers.markers.insert( - occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(), - collision_markers.end()); - } + std::vector collision_markers = + makeSlowDownMarkers(possible_collision, debug_data.road_type, id++); + occlusion_spot_slowdown_markers.markers.insert( + occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(), + collision_markers.end()); } // draw obstacle collision @@ -194,20 +221,8 @@ visualization_msgs::msg::MarkerArray createMarkers( occlusion_spot_slowdown_markers.markers.insert( occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(), collision_markers.end()); - break; - } - - // draw slice if having sidewalk polygon - if (!debug_data.sidewalks.empty()) { - id = 0; - for (const auto & sidewalk : debug_data.sidewalks) { - for (const visualization_msgs::msg::Marker & m : - makePolygonMarker(sidewalk, debug_data.z, id++)) { - occlusion_spot_slowdown_markers.markers.push_back(m); - } - } + id++; } - debug_data.sidewalks.clear(); return occlusion_spot_slowdown_markers; } @@ -227,6 +242,14 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMa visualization_msgs::msg::MarkerArray debug_marker_array; appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array); + appendMarkerArray( + makePolygonMarker(debug_data_.sidewalks, debug_data_.z), current_time, &debug_marker_array); + appendMarkerArray( + createPathMarkerArray(debug_data_.path_raw, "path_raw", 0, 0.0, 1.0, 1.0), current_time, + &debug_marker_array); + appendMarkerArray( + createPathMarkerArray(debug_data_.interp_path, "path_interp", 0, 0.0, 1.0, 1.0), current_time, + &debug_marker_array); return debug_marker_array; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 5a24c5bd4e9d5..dbad637972216 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -21,9 +21,11 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -63,10 +65,12 @@ bool splineInterpolate( std::vector base_x; std::vector base_y; std::vector base_z; + std::vector base_v; for (const auto & p : input.points) { base_x.push_back(p.point.pose.position.x); base_y.push_back(p.point.pose.position.y); base_z.push_back(p.point.pose.position.z); + base_v.push_back(p.point.longitudinal_velocity_mps); } std::vector base_s = interpolation::calcEuclidDist(base_x, base_y); @@ -80,6 +84,7 @@ bool splineInterpolate( base_x.erase(base_x.begin() + i); base_y.erase(base_y.begin() + i); base_z.erase(base_z.begin() + i); + base_v.erase(base_v.begin() + i); Ns -= 1; i -= 1; } @@ -96,14 +101,16 @@ bool splineInterpolate( const std::vector resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s); const std::vector resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s); const std::vector resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s); + const std::vector resampled_v = ::interpolation::slerp(base_s, base_v, resampled_s); // set xy output->points.clear(); for (size_t i = 0; i < resampled_s.size(); i++) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + PathPointWithLaneId p; p.point.pose.position.x = resampled_x.at(i); p.point.pose.position.y = resampled_y.at(i); p.point.pose.position.z = resampled_z.at(i); + p.point.longitudinal_velocity_mps = resampled_v.at(i); output->points.push_back(p); } @@ -115,9 +122,9 @@ bool splineInterpolate( output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); } if (output->points.size() > 1) { - size_t l = resampled_s.size(); + size_t l = output->points.size(); output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; - output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation; + output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation; } return true; } @@ -155,18 +162,9 @@ ROAD_TYPE getCurrentRoadType( return road_type; } -geometry_msgs::msg::Point setPoint(double x, double y, double z) -{ - geometry_msgs::msg::Point p; - p.x = x; - p.y = y; - p.z = z; - return p; -} - void calcSlowDownPointsForPossibleCollision( const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double offset_from_ego_to_target, std::vector & possible_collisions) + const double offset, std::vector & possible_collisions) { if (possible_collisions.empty()) { return; @@ -186,11 +184,11 @@ void calcSlowDownPointsForPossibleCollision( }; // insert path point orientation to possible collision size_t collision_index = 0; - double dist_along_path_point = offset_from_ego_to_target; + double dist_along_path_point = offset; double dist_along_next_path_point = dist_along_path_point; for (size_t idx = closest_idx; idx < path.points.size() - 1; idx++) { - auto p_prev = path.points[idx].point; - auto p_next = path.points[idx + 1].point; + auto p_prev = path.points.at(idx).point; + auto p_next = path.points.at(idx + 1).point; const double dist_to_col = possible_collisions.at(collision_index).arc_lane_dist_at_collision.length; dist_along_next_path_point += @@ -202,19 +200,17 @@ void calcSlowDownPointsForPossibleCollision( const double d1 = dist_along_next_path_point; const auto p0 = p_prev.pose.position; const auto p1 = p_next.pose.position; - auto & col = possible_collisions[collision_index]; - auto & v = col.collision_path_point.longitudinal_velocity_mps; - const double current_dist2col = col.arc_lane_dist_at_collision.length; - v = getInterpolatedValue( - d0, p_prev.longitudinal_velocity_mps, dist_to_col, d1, p_next.longitudinal_velocity_mps); - const double x = getInterpolatedValue(d0, p0.x, dist_to_col, d1, p1.x); - const double y = getInterpolatedValue(d0, p0.y, dist_to_col, d1, p1.y); + const double v0 = p_prev.longitudinal_velocity_mps; + const double v1 = p_next.longitudinal_velocity_mps; + const double v = getInterpolatedValue(d0, v0, dist_to_col, d1, v1); const double z = getInterpolatedValue(d0, p0.z, dist_to_col, d1, p1.z); // height is used to visualize marker correctly - - possible_collisions[collision_index].collision_path_point.pose.position = setPoint(x, y, z); - possible_collisions[collision_index].intersection_pose.position.z = z; - possible_collisions[collision_index].obstacle_info.position.z = z; + auto & col = possible_collisions.at(collision_index); + col.collision_path_point.longitudinal_velocity_mps = v; + col.collision_path_point.pose.position.z = z; + col.intersection_pose.position.z = z; + col.obstacle_info.position.z = z; + const double current_dist2col = col.arc_lane_dist_at_collision.length + offset; // break searching if dist to collision is farther than next path point if (dist_along_next_path_point < current_dist2col) { break; @@ -228,244 +224,211 @@ void calcSlowDownPointsForPossibleCollision( } } -autoware_auto_planning_msgs::msg::Path toPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) +std::vector getParkedVehicles( + const PredictedObjects & dyn_objects, const PlannerParam & param, + std::vector & debug_point) { - autoware_auto_planning_msgs::msg::Path path; - for (const auto & p : path_with_id.points) { - path.points.push_back(p.point); + std::vector parked_vehicles; + std::vector points; + for (const auto & obj : dyn_objects.objects) { + bool is_parked_vehicle = true; + if (!occlusion_spot_utils::isStuckVehicle(obj, param.stuck_vehicle_vel)) { + continue; + } + const geometry_msgs::msg::Point & p = obj.kinematics.initial_pose_with_covariance.pose.position; + BasicPoint2d obj_point(p.x, p.y); + if (is_parked_vehicle) { + parked_vehicles.emplace_back(obj); + points.emplace_back(p); + } } - return path; + debug_point = points; + return parked_vehicles; } -lanelet::ConstLanelet buildPathLanelet( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string) { - autoware_auto_planning_msgs::msg::Path converted_path = filterLitterPathPoint(toPath(path)); - if (converted_path.points.empty()) { - return lanelet::ConstLanelet(); - } - // interpolation is done previously - lanelet::BasicLineString2d path_line; - path_line.reserve(converted_path.points.size()); - // skip last point that will creates extra ordinary path - for (size_t i = 0; i < converted_path.points.size() - 1; ++i) { - const auto & pose_i = converted_path.points[i].pose; - path_line.emplace_back(pose_i.position.x, pose_i.position.y); + Polygon2d poly = planning_utils::toFootprintPolygon(obj); + std::deque arcs; + for (const auto & p : poly.outer()) { + lanelet::BasicPoint2d obj_p = {p.x(), p.y()}; + arcs.emplace_back(lanelet::geometry::toArcCoordinates(ll_string, obj_p)); } - // set simplified line to lanelet - lanelet::BasicLineString2d simplified_line; - boost::geometry::simplify(path_line, simplified_line, 0.1); - lanelet::Points3d path_points; - path_points.reserve(simplified_line.size()); - for (const auto & p : simplified_line) { - path_points.emplace_back(lanelet::InvalId, p.x(), p.y(), 0.0); + /** remove + * x--------* + * | | + * x--------* <- return + * Ego===============> path + **/ + // sort by arc length + std::sort(arcs.begin(), arcs.end(), [](ArcCoordinates arc1, ArcCoordinates arc2) { + return arc1.length < arc2.length; + }); + // remove closest 2 polygon point + arcs.pop_front(); + arcs.pop_front(); + // sort by arc distance + std::sort(arcs.begin(), arcs.end(), [](ArcCoordinates arc1, ArcCoordinates arc2) { + return std::abs(arc1.distance) < std::abs(arc2.distance); + }); + // return closest to path point which is choosen by farthest 2 points. + return arcs.at(0); +} + +// calculate value removing offset distance or 0 +double calcSignedLateralDistanceWithOffset(const double lateral, const double offset) +{ + // if distance is lower than offset return 0; + if (std::abs(lateral) < offset) { + return 0; + } else if (lateral < 0) { + return lateral + offset; + } else { + return lateral - offset; } - lanelet::LineString3d centerline(lanelet::InvalId, path_points); - lanelet::Lanelet path_lanelet(lanelet::InvalId); - path_lanelet.setCenterline(centerline); - return lanelet::ConstLanelet(path_lanelet); + // error case + return -1.0; } -void calculateCollisionPathPointFromOcclusionSpot( - PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point, - const double offset_from_ego_to_target, const lanelet::ConstLanelet & path_lanelet, - const PlannerParam & param) +PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( + const ArcCoordinates & arc_coord_occlusion, + const ArcCoordinates & arc_coord_occlusion_with_offset, + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { - auto calcSignedArcDistance = [](const double lateral_distance, const double offset) { - if (lateral_distance < 0) { - return lateral_distance + offset; - } - return lateral_distance - offset; + auto setPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) { + const auto & ll = pl.centerline2d(); + BasicPoint2d bp = fromArcCoordinates(ll, arc); + Pose pose; + pose.position.x = bp[0]; + pose.position.y = bp[1]; + pose.position.z = 0.0; + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + lanelet::utils::getLaneletAngle(pl, pose.position)); + return pose; }; - lanelet::ArcCoordinates arc_lane_occlusion_point = - lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obstacle_point); - lanelet::BasicPoint2d intersection_point = lanelet::geometry::fromArcCoordinates( - path_lanelet.centerline2d(), {arc_lane_occlusion_point.length, 0.0}); - lanelet::BasicPoint2d col_point = lanelet::geometry::fromArcCoordinates( - path_lanelet.centerline2d(), - {arc_lane_occlusion_point.length - param.vehicle_info.baselink_to_front, 0.0}); - geometry_msgs::msg::Point search_point; - search_point.x = col_point[0]; - search_point.y = col_point[1]; - search_point.z = 0; - double path_angle = lanelet::utils::getLaneletAngle(path_lanelet, search_point); - tf2::Quaternion quat; - quat.setRPY(0, 0, path_angle); - autoware_auto_planning_msgs::msg::PathPoint collision_path_point; - pc.collision_path_point.pose.position.x = col_point[0]; - pc.collision_path_point.pose.position.y = col_point[1]; - pc.collision_path_point.pose.orientation = tf2::toMsg(quat); - ObstacleInfo obstacle_info; - pc.obstacle_info.position.x = obstacle_point[0]; - pc.obstacle_info.position.y = obstacle_point[1]; + /** + * @brief calculate obstacle collision intersection from arc coordinate info. + * ^ + * | + * Ego ---------collision----------intersection-------> path + * | + * ------------------ | + * | Vehicle | obstacle + * ------------------ + */ + PossibleCollisionInfo pc; + pc.arc_lane_dist_at_collision = arc_coord_occlusion_with_offset; + pc.obstacle_info.position = setPose(path_lanelet, arc_coord_occlusion).position; pc.obstacle_info.max_velocity = param.pedestrian_vel; - geometry_msgs::msg::Pose intersection_pose; - pc.intersection_pose.position.x = intersection_point[0]; - pc.intersection_pose.position.y = intersection_point[1]; - pc.intersection_pose.orientation = tf2::toMsg(quat); - double signed_lateral_distance = calcSignedArcDistance( - arc_lane_occlusion_point.distance, param.vehicle_info.vehicle_width * 0.5); - pc.arc_lane_dist_at_collision = { - arc_lane_occlusion_point.length + offset_from_ego_to_target - - param.vehicle_info.baselink_to_front, - std::abs(signed_lateral_distance)}; + pc.collision_path_point.pose = + setPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0}); + pc.intersection_pose = setPose(path_lanelet, {arc_coord_occlusion.length, 0.0}); + return pc; } -void createPossibleCollisionBehindParkedVehicle( - std::vector & possible_collisions, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PlannerParam & param, - const double offset_from_ego_to_target, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & dyn_obj_arr) +std::vector generatePossibleCollisionBehindParkedVehicle( + const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, + const std::vector & dyn_objects) { - lanelet::ConstLanelet path_lanelet = buildPathLanelet(path); - if (path_lanelet.centerline2d().empty()) { - return; - } - std::vector arcs; - auto calcSignedArcDistance = [](const double lateral_distance, const double offset) { - if (lateral_distance < 0) { - return lateral_distance + offset; - } - return lateral_distance - offset; - }; + lanelet::ConstLanelet path_lanelet = toPathLanelet(path); + std::vector possible_collisions; const double half_vehicle_width = 0.5 * param.vehicle_info.vehicle_width; - for (const auto & dyn : dyn_obj_arr->objects) { - // consider if dynamic object is a car or bus or truck + const double baselink_to_front = param.vehicle_info.baselink_to_front; + auto ll = path_lanelet.centerline2d(); + for (const auto & dyn : dyn_objects) { + ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll); + ArcCoordinates arc_coord_occlusion_with_offset = { + arc_coord_occlusion.length - baselink_to_front, + calcSignedLateralDistanceWithOffset(arc_coord_occlusion.distance, half_vehicle_width)}; + // ignore if collision is not avoidable by velocity control. if ( - dyn.classification.at(0).label != - autoware_auto_perception_msgs::msg::ObjectClassification::CAR && - dyn.classification.at(0).label != - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && - dyn.classification.at(0).label != - autoware_auto_perception_msgs::msg::ObjectClassification::BUS) { + arc_coord_occlusion_with_offset.length < offset_from_start_to_ego || + arc_coord_occlusion_with_offset.length > param.detection_area_length || + arc_coord_occlusion_with_offset.length > lanelet::geometry::length2d(path_lanelet) || + std::abs(arc_coord_occlusion_with_offset.distance) <= 1e-3 || + std::abs(arc_coord_occlusion_with_offset.distance) > param.lateral_distance_thr) { continue; } - const auto & state = dyn.kinematics; - // ignore if vehicle is moving - if (state.initial_twist_with_covariance.twist.linear.x > param.stuck_vehicle_vel) { - continue; - } - const geometry_msgs::msg::Point & p = dyn.kinematics.initial_pose_with_covariance.pose.position; - const geometry_msgs::msg::Quaternion & q = - dyn.kinematics.initial_pose_with_covariance.pose.orientation; - lanelet::BasicPoint2d obj_point; - obj_point[0] = p.x; - obj_point[1] = p.y; - lanelet::ArcCoordinates arc_lane_point_at_occlusion = - lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obj_point); - // add a half size of car to arc length as behind car - arc_lane_point_at_occlusion.length += dyn.shape.dimensions.x * 0.5; - // signed lateral distance used to convert obstacle position - double signed_lateral_distance = - calcSignedArcDistance(arc_lane_point_at_occlusion.distance, half_vehicle_width); - arc_lane_point_at_occlusion.distance = - std::abs(arc_lane_point_at_occlusion.distance) - 0.5 * dyn.shape.dimensions.y; - // ignore if obstacle is not within attention area - if ( - offset_from_ego_to_target + arc_lane_point_at_occlusion.length < - param.vehicle_info.baselink_to_front || - arc_lane_point_at_occlusion.distance < half_vehicle_width || - param.lateral_distance_thr + half_vehicle_width < arc_lane_point_at_occlusion.distance) { - continue; - } - // subtract baselink_to_front from longitudinal distance to occlusion point - double longitudinal_dist_at_collision_point = - arc_lane_point_at_occlusion.length - param.vehicle_info.baselink_to_front; - // collision point at baselink - lanelet::BasicPoint2d collision_point = lanelet::geometry::fromArcCoordinates( - path_lanelet.centerline2d(), {longitudinal_dist_at_collision_point, 0.0}); - geometry_msgs::msg::Point search_point; - search_point.x = collision_point[0]; - search_point.y = collision_point[1]; - search_point.z = 0; - double path_angle = lanelet::utils::getLaneletAngle(path_lanelet, search_point); - // ignore if angle is more different than 10[degree] - double obj_angle = tf2::getYaw(q); - const double diff_angle = tier4_autoware_utils::normalizeRadian(path_angle - obj_angle); - if (std::abs(diff_angle) > param.angle_thr) { - continue; - } - lanelet::BasicPoint2d obstacle_point = lanelet::geometry::fromArcCoordinates( - path_lanelet.centerline2d(), {arc_lane_point_at_occlusion.length, signed_lateral_distance}); - PossibleCollisionInfo pc; - calculateCollisionPathPointFromOcclusionSpot( - pc, obstacle_point, offset_from_ego_to_target, path_lanelet, param); + PossibleCollisionInfo pc = calculateCollisionPathPointFromOcclusionSpot( + arc_coord_occlusion, arc_coord_occlusion_with_offset, path_lanelet, param); possible_collisions.emplace_back(pc); } + // sort by arc length + std::sort( + possible_collisions.begin(), possible_collisions.end(), + [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { + return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; + }); + return possible_collisions; } -bool extractTargetRoad( - const int closest_idx, const lanelet::LaneletMapPtr lanelet_map_ptr, const double max_range, - const autoware_auto_planning_msgs::msg::PathWithLaneId & src_path, - double & offset_from_closest_to_target, - autoware_auto_planning_msgs::msg::PathWithLaneId & tar_path, const ROAD_TYPE & target_road_type) +DetectionAreaIdx extractTargetRoadArcLength( + const lanelet::LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path, + const ROAD_TYPE & target_road_type) { bool found_target = false; + double start_dist = 0.0; + double dist_sum = 0.0; // search lanelet that includes target_road_type only - for (size_t i = closest_idx; i < src_path.points.size(); i++) { - occlusion_spot_utils::ROAD_TYPE search_road_type = occlusion_spot_utils::getCurrentRoadType( - lanelet_map_ptr->laneletLayer.get(src_path.points[i].lane_ids[0]), lanelet_map_ptr); + for (size_t i = 0; i < path.points.size() - 1; i++) { + ROAD_TYPE search_road_type = occlusion_spot_utils::getCurrentRoadType( + lanelet_map_ptr->laneletLayer.get(path.points[i].lane_ids[0]), lanelet_map_ptr); if (found_target && search_road_type != target_road_type) { break; } // ignore path farther than max range - if (offset_from_closest_to_target > max_range) { + if (dist_sum > max_range) { break; } - if (search_road_type == target_road_type) { - tar_path.points.emplace_back(src_path.points[i]); + if (!found_target && search_road_type == target_road_type) { + start_dist = dist_sum; found_target = true; } - if (!found_target && i < src_path.points.size() - 1) { - const auto & curr_p = src_path.points[i].point.pose.position; - const auto & next_p = src_path.points[i + 1].point.pose.position; - offset_from_closest_to_target += tier4_autoware_utils::calcDistance2d(curr_p, next_p); - } + const auto & curr_p = path.points[i].point.pose.position; + const auto & next_p = path.points[i + 1].point.pose.position; + dist_sum += tier4_autoware_utils::calcDistance2d(curr_p, next_p); } - return found_target; + if (!found_target) return {}; + return DetectionAreaIdx(std::make_pair(start_dist, dist_sum)); } -void generatePossibleCollisions( - std::vector & possible_collisions, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const grid_map::GridMap & grid, - const double offset_from_ego_to_closest, const double offset_from_closest_to_target, - const PlannerParam & param, std::vector & debug) +void filterCollisionByRoadType( + std::vector & possible_collisions, const DetectionAreaIdx area) { - // NOTE : buildPathLanelet first index should always be zero because path is already limited - lanelet::ConstLanelet path_lanelet = toPathLanelet(path); - if (path_lanelet.centerline2d().empty()) { - return; + std::pair focus_length = area.get(); + for (auto it = possible_collisions.begin(); it != possible_collisions.end();) { + const auto & pc_len = it->arc_lane_dist_at_collision.length; + if (focus_length.first < pc_len && pc_len < focus_length.second) { + it++; + } else { + // -----erase-----|start------target-------end|----erase--- + it = possible_collisions.erase(it); + } } - // generate sidewalk possible collision - generateSidewalkPossibleCollisions( - possible_collisions, grid, offset_from_ego_to_closest, offset_from_closest_to_target, - path_lanelet, param, debug); - possible_collisions.insert( - possible_collisions.end(), possible_collisions.begin(), possible_collisions.end()); } void generateSidewalkPossibleCollisions( std::vector & possible_collisions, const grid_map::GridMap & grid, - const double offset_from_ego_to_closest, const double offset_from_closest_to_target, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, + const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, std::vector & debug) { - const double offset_form_ego_to_target = - offset_from_ego_to_closest + offset_from_closest_to_target; - double min_range = 0; - // if ego is inside the target path_lanelet, set ignore_length - if (offset_form_ego_to_target > param.vehicle_info.baselink_to_front) { - min_range = 0; - } else { - min_range = param.vehicle_info.baselink_to_front - offset_form_ego_to_target; + lanelet::ConstLanelet path_lanelet = toPathLanelet(path); + if (path_lanelet.centerline2d().empty()) { + return; } std::vector sidewalk_slices; geometry::buildSidewalkSlices( - sidewalk_slices, path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5, + sidewalk_slices, path_lanelet, 0.0, param.vehicle_info.vehicle_width * 0.5, param.sidewalk.slice_size, param.sidewalk.focus_range); double length_lower_bound = std::numeric_limits::max(); double distance_lower_bound = std::numeric_limits::max(); + // sort distance closest first to skip inferior collision + std::sort( + sidewalk_slices.begin(), sidewalk_slices.end(), + [](const geometry::Slice & s1, const geometry::Slice & s2) { + return std::abs(s1.range.min_distance) < std::abs(s2.range.min_distance); + }); + std::sort( sidewalk_slices.begin(), sidewalk_slices.end(), [](const geometry::Slice & s1, const geometry::Slice & s2) { @@ -481,8 +444,8 @@ void generateSidewalkPossibleCollisions( occlusion_spot_positions, grid, sidewalk_slice.polygon, param.sidewalk.min_occlusion_spot_size); generateSidewalkPossibleCollisionFromOcclusionSpot( - possible_collisions, grid, occlusion_spot_positions, offset_form_ego_to_target, - path_lanelet, param); + possible_collisions, grid, occlusion_spot_positions, offset_from_start_to_ego, path_lanelet, + param); if (!possible_collisions.empty()) { length_lower_bound = sidewalk_slice.range.min_length; distance_lower_bound = std::abs(sidewalk_slice.range.min_distance); @@ -497,29 +460,40 @@ void generateSidewalkPossibleCollisions( void generateSidewalkPossibleCollisionFromOcclusionSpot( std::vector & possible_collisions, const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, - const double offset_form_ego_to_target, const lanelet::ConstLanelet & path_lanelet, + const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { + const double baselink_to_front = param.vehicle_info.baselink_to_front; + const double half_vehicle_width = param.vehicle_info.vehicle_width * 0.5; + double distance_lower_bound = std::numeric_limits::max(); + PossibleCollisionInfo candidate; + bool has_collision = false; for (grid_map::Position occlusion_spot_position : occlusion_spot_positions) { // arc intersection lanelet::BasicPoint2d obstacle_point = {occlusion_spot_position[0], occlusion_spot_position[1]}; - lanelet::ArcCoordinates arc_lane_point_at_occlusion = + lanelet::ArcCoordinates arc_coord_occlusion_point = lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obstacle_point); - lanelet::BasicPoint2d intersection_point = lanelet::geometry::fromArcCoordinates( - path_lanelet.centerline2d(), {arc_lane_point_at_occlusion.length, 0.0}); - bool collision_free = - grid_utils::isCollisionFree(grid, occlusion_spot_position, intersection_point); - if (collision_free) { - PossibleCollisionInfo pc; - calculateCollisionPathPointFromOcclusionSpot( - pc, obstacle_point, offset_form_ego_to_target, path_lanelet, param); - if (pc.arc_lane_dist_at_collision.length < param.vehicle_info.baselink_to_front) { - continue; - } - possible_collisions.emplace_back(pc); - break; + const double length_to_col = arc_coord_occlusion_point.length - baselink_to_front; + ArcCoordinates arc_coord_collision_point = { + length_to_col, + calcSignedLateralDistanceWithOffset(arc_coord_occlusion_point.distance, half_vehicle_width)}; + if (length_to_col < offset_from_start_to_ego) { + continue; + } + PossibleCollisionInfo pc = calculateCollisionPathPointFromOcclusionSpot( + arc_coord_occlusion_point, arc_coord_collision_point, path_lanelet, param); + const auto & ip = pc.intersection_pose.position; + bool collision_free_at_intersection = + grid_utils::isCollisionFree(grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y)); + // this is going to extract collision that is nearest to path + if ( + collision_free_at_intersection && distance_lower_bound > arc_coord_collision_point.distance) { + distance_lower_bound = arc_coord_collision_point.distance; + candidate = {pc}; + has_collision = true; } } + if (has_collision) possible_collisions.emplace_back(candidate); } } // namespace occlusion_spot_utils diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index de948e9c90259..21c78e5345592 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -37,7 +37,7 @@ void applySafeVelocityConsideringPossibleCollison( for (auto & possible_collision : possible_collisions) { const double dist_to_collision = possible_collision.arc_lane_dist_at_collision.length; const double original_vel = possible_collision.collision_path_point.longitudinal_velocity_mps; - const double d_obs = possible_collision.arc_lane_dist_at_collision.distance; + const double d_obs = std::abs(possible_collision.arc_lane_dist_at_collision.distance); const double v_obs = possible_collision.obstacle_info.max_velocity; // skip if obstacle velocity is below zero if (v_obs < 0) { diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp index e5becaf870ba6..ffc53f8efa586 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp @@ -25,6 +25,11 @@ namespace behavior_velocity_planner { +using occlusion_spot_utils::ROAD_TYPE::PRIVATE; +namespace bg = boost::geometry; +namespace lg = lanelet::geometry; +namespace utils = occlusion_spot_utils; + OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, @@ -39,6 +44,7 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { + debug_data_ = DebugData(); if (path->points.size() < 2) { return true; } @@ -54,58 +60,49 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( return true; } + const double max_range = 50.0; + PathWithLaneId clipped_path; + utils::clipPathByLength(*path, clipped_path, max_range); + PathWithLaneId interp_path; + utils::splineInterpolate(clipped_path, 1.0, &interp_path, logger_); + debug_data_.interp_path = interp_path; int closest_idx = -1; - if (!planning_utils::calcClosestIndex( - *path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { - return true; - } - - const auto target_road_type = occlusion_spot_utils::ROAD_TYPE::PRIVATE; - autoware_auto_planning_msgs::msg::PathWithLaneId limited_path; - double offset_from_ego_to_closest = 0; - double offset_from_closest_to_target = 0; - const double max_range = occ_grid_ptr->info.width * occ_grid_ptr->info.resolution; - { - // extract lanelet that includes target_road_type only - if (!behavior_velocity_planner::occlusion_spot_utils::extractTargetRoad( - closest_idx, lanelet_map_ptr, max_range, *path, offset_from_closest_to_target, - limited_path, target_road_type)) { - return true; - } - // use path point as origin for stability - offset_from_ego_to_closest = - -planning_utils::transformRelCoordinate2D(ego_pose, path->points[closest_idx].point.pose) - .position.x; - } - autoware_auto_planning_msgs::msg::PathWithLaneId interp_path; - occlusion_spot_utils::splineInterpolate(limited_path, 1.0, &interp_path, logger_); - if (interp_path.points.size() < 4) { + if (!planning_utils::calcClosestIndex( + interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { return true; } + // return if ego is final point of interpolated path + if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; + DetectionAreaIdx focus_area = + extractTargetRoadArcLength(lanelet_map_ptr, max_range, *path, PRIVATE); + if (!focus_area) return true; nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; grid_map::GridMap grid_map; grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); if (param_.show_debug_grid) { publisher_->publish(occ_grid); } - std::vector possible_collisions; - const double offset_from_ego_to_target = - offset_from_ego_to_closest + offset_from_closest_to_target; + double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 3000, "offset_from_ego_to_target : " << offset_from_ego_to_target); - occlusion_spot_utils::generatePossibleCollisions( - possible_collisions, interp_path, grid_map, offset_from_ego_to_closest, - offset_from_closest_to_target, param_, debug_data_.sidewalks); + logger_, *clock_, 3000, "offset_from_start_to_ego : " << offset_from_start_to_ego); + std::vector possible_collisions; + // Note: Don't consider offset from path start to ego here + utils::generateSidewalkPossibleCollisions( + possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, + debug_data_.sidewalks); + utils::filterCollisionByRoadType(possible_collisions, focus_area); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); - behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision( - closest_idx, *path, offset_from_ego_to_target, possible_collisions); + utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); + // Note: Consider offset from path start to ego here + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); // apply safe velocity using ebs and pbs deceleration applySafeVelocityConsideringPossibleCollison( path, possible_collisions, ego_velocity, param_.private_road, param_); debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; + debug_data_.path_raw = *path; return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index 2456fa9b2f895..4d29fdb0dd93e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -21,10 +21,16 @@ #include #include +#include +#include #include namespace behavior_velocity_planner { +using occlusion_spot_utils::PossibleCollisionInfo; +using occlusion_spot_utils::ROAD_TYPE::PUBLIC; +namespace utils = occlusion_spot_utils; + OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, @@ -38,6 +44,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { + debug_data_ = DebugData(); if (path->points.size() < 2) { return true; } @@ -47,49 +54,38 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; const double ego_velocity = planner_data_->current_velocity->twist.linear.x; const auto & lanelet_map_ptr = planner_data_->lanelet_map; + const auto & routing_graph_ptr = planner_data_->routing_graph; const auto & traffic_rules_ptr = planner_data_->traffic_rules; const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - if (!lanelet_map_ptr || !traffic_rules_ptr || !dynamic_obj_arr_ptr) { + + if (!lanelet_map_ptr || !traffic_rules_ptr || !dynamic_obj_arr_ptr || !routing_graph_ptr) { return true; } + PathWithLaneId clipped_path; + utils::clipPathByLength(*path, clipped_path, param_.detection_area_length); + PathWithLaneId interp_path; + utils::splineInterpolate(clipped_path, 1.0, &interp_path, logger_); int closest_idx = -1; - if (!planning_utils::calcClosestIndex( - *path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { - return true; - } - const auto target_road_type = occlusion_spot_utils::ROAD_TYPE::PUBLIC; - autoware_auto_planning_msgs::msg::PathWithLaneId limited_path; - double offset_from_ego_to_closest = 0; - double offset_from_closest_to_target = 0; - { - // extract lanelet that includes target_road_type only - if (!behavior_velocity_planner::occlusion_spot_utils::extractTargetRoad( - closest_idx, lanelet_map_ptr, param_.detection_area_length, *path, - offset_from_closest_to_target, limited_path, target_road_type)) { - return true; - } - // use path point as origin for stability - offset_from_ego_to_closest = - -planning_utils::transformRelCoordinate2D(ego_pose, path->points[closest_idx].point.pose) - .position.x; - } - autoware_auto_planning_msgs::msg::PathWithLaneId interp_path; - occlusion_spot_utils::splineInterpolate(limited_path, 1.0, &interp_path, logger_); - if (interp_path.points.size() < 4) { + if (!planning_utils::calcClosestIndex( + interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { return true; } - std::vector - possible_collisions; - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 3000, "offset_from_ego_to_closest : " << offset_from_ego_to_closest); - const double offset_from_ego_to_target = - offset_from_ego_to_closest + offset_from_closest_to_target; - behavior_velocity_planner::occlusion_spot_utils::createPossibleCollisionBehindParkedVehicle( - possible_collisions, interp_path, param_, offset_from_ego_to_target, dynamic_obj_arr_ptr); - // set orientation to each possible collision - behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision( - closest_idx, *path, offset_from_ego_to_target, possible_collisions); + // return if ego is final point of interpolated path + if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; + DetectionAreaIdx focus_area = + extractTargetRoadArcLength(lanelet_map_ptr, param_.detection_area_length, *path, PUBLIC); + if (!focus_area) return true; + std::vector obj = + utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); + double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); + // Note: Don't consider offset from path start to ego here + std::vector possible_collisions = + utils::generatePossibleCollisionBehindParkedVehicle( + interp_path, param_, offset_from_start_to_ego, obj); + utils::filterCollisionByRoadType(possible_collisions, focus_area); + utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); + // Note: Consider offset from path start to ego here + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); // apply safe velocity using ebs and pbs deceleration applySafeVelocityConsideringPossibleCollison( path, possible_collisions, ego_velocity, param_.public_road, param_); diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index a94ec5eab3c97..c637ffbd73ef0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -70,6 +71,8 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec", 3.0); p.near_line_distance = node.declare_parameter(ns + ".near_line_distance", 1.0); p.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 1.0); + p.max_yaw_deviation_rad = + tier4_autoware_utils::deg2rad(node.declare_parameter(ns + ".max_yaw_deviation_deg", 90.0)); p.check_timeout_after_stop_line = node.declare_parameter(ns + ".check_timeout_after_stop_line", true); } diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index fcccaca11eee3..5c0723836da9a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -19,9 +19,20 @@ #include #include +#include #include #include +namespace tier4_autoware_utils +{ +template <> +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose; +} +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { namespace @@ -182,7 +193,7 @@ SegmentIndexWithOffset findBackwardOffsetSegment( const double offset_length) { double sum_length = 0.0; - for (size_t i = base_idx - 1; i > 0; --i) { + for (size_t i = base_idx; i > 0; --i) { const auto p_front = path.points.at(i - 1); const auto p_back = path.points.at(i); @@ -338,6 +349,9 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( command_.type = map_data_.instrument_type; command_.id = map_data_.instrument_id; command_.custom_tags = map_data_.custom_tags; + + // Add instrument information to the logger + logger_ = logger_.get_child((map_data_.instrument_type + "_" + map_data_.instrument_id).c_str()); } bool VirtualTrafficLightModule::modifyPathVelocity( @@ -399,10 +413,10 @@ bool VirtualTrafficLightModule::modifyPathVelocity( return true; } - // Stop at stop_line if state is timeout - if (isBeforeStopLine() || planner_param_.check_timeout_after_stop_line) { + // Stop at stop_line if state is timeout before stop_line + if (isBeforeStopLine()) { if (isStateTimeout(*virtual_traffic_light_state)) { - RCLCPP_DEBUG(logger_, "state is timeout"); + RCLCPP_DEBUG(logger_, "state is timeout before stop line"); insertStopVelocityAtStopLine(path, stop_reason); } @@ -413,6 +427,15 @@ bool VirtualTrafficLightModule::modifyPathVelocity( // After stop_line state_ = State::PASSING; + // Check timeout after stop_line if the option is on + if ( + planner_param_.check_timeout_after_stop_line && isStateTimeout(*virtual_traffic_light_state)) { + RCLCPP_DEBUG(logger_, "state is timeout after stop line"); + insertStopVelocityAtStopLine(path, stop_reason); + updateInfrastructureCommand(); + return true; + } + // Stop at stop_line if finalization isn't completed if (!virtual_traffic_light_state->is_finalized) { RCLCPP_DEBUG(logger_, "finalization isn't completed"); @@ -453,10 +476,12 @@ bool VirtualTrafficLightModule::isBeforeStartLine() return false; } + const double max_dist = std::numeric_limits::max(); const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose.position, collision->point); + module_data_.path.points, module_data_.head_pose, collision->point, max_dist, + planner_param_.max_yaw_deviation_rad); - return signed_arc_length > 0; + return *signed_arc_length > 0; } bool VirtualTrafficLightModule::isBeforeStopLine() @@ -469,10 +494,12 @@ bool VirtualTrafficLightModule::isBeforeStopLine() return false; } + const double max_dist = std::numeric_limits::max(); const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose.position, collision->point); + module_data_.path.points, module_data_.head_pose, collision->point, max_dist, + planner_param_.max_yaw_deviation_rad); - return signed_arc_length > -planner_param_.dead_line_margin; + return *signed_arc_length > -planner_param_.dead_line_margin; } bool VirtualTrafficLightModule::isAfterAnyEndLine() @@ -484,16 +511,18 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine() const auto collision = findCollision(module_data_.path.points, map_data_.end_lines); - // Since the module is registered, a collision should be detected usually. - // Therefore if no collision found, vehicle's path is fully after the line. + // If the goal is set before the end line, collision will not be detected. + // Therefore if there is no collision, the ego vehicle is assumed to be before the end line. if (!collision) { return false; } + const double max_dist = std::numeric_limits::max(); const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose.position, collision->point); + module_data_.path.points, module_data_.head_pose, collision->point, max_dist, + planner_param_.max_yaw_deviation_rad); - return signed_arc_length < -planner_param_.dead_line_margin; + return *signed_arc_length < -planner_param_.dead_line_margin; } bool VirtualTrafficLightModule::isNearAnyEndLine() @@ -504,10 +533,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine() return false; } + const double max_dist = std::numeric_limits::max(); const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength( - module_data_.path.points, module_data_.head_pose.position, collision->point); + module_data_.path.points, module_data_.head_pose, collision->point, max_dist, + planner_param_.max_yaw_deviation_rad); - return std::abs(signed_arc_length) < planner_param_.near_line_distance; + return std::abs(*signed_arc_length) < planner_param_.near_line_distance; } boost::optional diff --git a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp index 9de63c43c0a21..fa5430e2329b5 100644 --- a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp @@ -12,15 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "gtest/gtest.h" +#include "scene_module/occlusion_spot/occlusion_spot_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "utilization/path_utilization.hpp" +#include "utilization/util.hpp" #include "utils.hpp" -#include -#include +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/vector3.hpp" -#include - -#include -#include +using Point = geometry_msgs::msg::Point; +using Vector3 = geometry_msgs::msg::Vector3; +using DynamicObjects = autoware_auto_perception_msgs::msg::PredictedObjects; +using DynamicObject = autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathWithLaneId; autoware_auto_planning_msgs::msg::Path toPath( const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) @@ -46,28 +54,7 @@ TEST(spline, splineInterpolate) ASSERT_EQ(path_interp.points.size(), path.points.size() * 2 - +1); } -TEST(buildPathLanelet, nominal) -{ - using behavior_velocity_planner::occlusion_spot_utils::buildPathLanelet; - lanelet::ConstLanelet path_lanelet; - /* straight diagonal path - 0 1 2 3 4 - 0 x - 1 x - 2 x - 3 x - 4 x - */ - autoware_auto_planning_msgs::msg::PathWithLaneId path = test::generatePath(0, 0, 4, 4, 5); - path_lanelet = buildPathLanelet(path); - ASSERT_EQ(path_lanelet.centerline2d().front().x(), 0.0); - ASSERT_EQ(path_lanelet.centerline2d().front().y(), 0.0); - ASSERT_NE(path_lanelet.centerline2d().back().x(), 4.0); - ASSERT_NE(path_lanelet.centerline2d().back().y(), 4.0); - std::cout << "path lanelet size: " << path_lanelet.centerline2d().size() << std::endl; -} - -TEST(calcSlowDownPointsForPossibleCollision, ManyPossibleCollisions) +TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; @@ -109,108 +96,41 @@ TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) using std::chrono::duration_cast; using std::chrono::high_resolution_clock; using std::chrono::microseconds; - std::vector possible_collisions; - const double offset = 2; - autoware_auto_planning_msgs::msg::PathWithLaneId path = - test::generatePath(offset, 3.0, 6.0, 3.0, 5); - test::generatePossibleCollisions(possible_collisions, 3.0, 3.0, 6.0, 3.0, 3); - for (size_t i = 0; i < path.points.size(); i++) { - path.points[i].point.longitudinal_velocity_mps = static_cast(i + offset); + std::vector pcs; + // for public + { + const double offset_from_start_to_ego = 0; + PathWithLaneId path = test::generatePath(0.0, 3.0, 6.0, 3.0, 7); + for (size_t i = 0; i < path.points.size(); i++) { + path.points[i].point.longitudinal_velocity_mps = static_cast(i); + } + test::generatePossibleCollisions(pcs, 3.0, 3.0, 6.0, 3.0, 3); + /** + * @brief generated path and possible collisions : path start from 2 to 6 + * 0 1 2 3 4 5 6 + * x: e-p-p-p-p-p-p-> path + * v: 0-1-2-3-4-5-6-> velocity + * c: N-N-N-c-NcN-c-> collision + * --->| longitudinal offset + * e : ego + * p : path + * c : collision + */ + calcSlowDownPointsForPossibleCollision(0, path, -offset_from_start_to_ego, pcs); + if (pcs[0].collision_path_point.longitudinal_velocity_mps - 3.0 > 1e-3) { + for (size_t i = 0; i < path.points.size(); i++) { + std::cout << "v : " << path.points[i].point.longitudinal_velocity_mps << "\t"; + } + std::cout << std::endl; + for (const auto pc : pcs) { + std::cout << "len : " << pc.arc_lane_dist_at_collision.length << "\t"; + } + std::cout << std::endl; + } + EXPECT_DOUBLE_EQ(pcs[0].collision_path_point.longitudinal_velocity_mps, 3); + EXPECT_DOUBLE_EQ(pcs[1].collision_path_point.longitudinal_velocity_mps, 4.5); + EXPECT_DOUBLE_EQ(pcs[2].collision_path_point.longitudinal_velocity_mps, 6); } - /** - * @brief generated path and possible collisions : path start from 2 to 6 - * 0 1 2 3 4 5 6 - * x: e-n-n-p-p-p-p-> path - * v: N-N-2-3-4-5-6-> velocity - * c: N-N-N-c-NcN-c-> collision - * --->| longitudinal offset - * e : ego - * p : path - * c : collision - */ - - calcSlowDownPointsForPossibleCollision(0, path, offset, possible_collisions); - EXPECT_EQ(possible_collisions[0].collision_path_point.longitudinal_velocity_mps, 3); - EXPECT_EQ(possible_collisions[1].collision_path_point.longitudinal_velocity_mps, 4.5); - EXPECT_EQ(possible_collisions[2].collision_path_point.longitudinal_velocity_mps, 6); -} - -TEST(createPossibleCollisionBehindParkedVehicle, PathPointsAndObstacles) -{ - using behavior_velocity_planner::occlusion_spot_utils::createPossibleCollisionBehindParkedVehicle; - using std::chrono::duration; - using std::chrono::duration_cast; - using std::chrono::high_resolution_clock; - using std::chrono::microseconds; - - // make a path with 200 points from x=0 to x=200 - autoware_auto_planning_msgs::msg::PathWithLaneId path = - test::generatePath(0.0, 3.0, 200.0, 3.0, 100); - // There is a parked bus,car,truck along with ego path. - // Ignore vehicle dimensions to simplify test - std::cout << " 6 - |TRU| | | -> truck is ignored because of lateral distance \n" - << " 5 - | | | | \n" - << " 4 - |CAR| | | -> considered \n" - << " 3Ego--|---|--path--> (2000 points) \n" - << " ＝＝median strip==== \n" - << " 2 - | | |SUB| -> bus is ignored because of opposite direction \n" - << " 1 - | | | | \n" - << " 0 | 1 | 2 | 3 | 4 | \n"; - - autoware_auto_perception_msgs::msg::PredictedObjects obj_arr; - autoware_auto_perception_msgs::msg::PredictedObject obj; - obj.shape.dimensions.x = 0.0; - obj.shape.dimensions.y = 0.0; - obj.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(0.0); - obj.kinematics.initial_twist_with_covariance.twist.linear.x = 0; - obj.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); - - // car - obj.kinematics.initial_pose_with_covariance.pose.position.x = 2.5; - obj.kinematics.initial_pose_with_covariance.pose.position.y = 4.0; - obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; - const size_t num_car = 3; - for (size_t i = 0; i < num_car; i++) { - obj_arr.objects.emplace_back(obj); - } - - // truck - obj.kinematics.initial_pose_with_covariance.pose.position.x = 2.5; - obj.kinematics.initial_pose_with_covariance.pose.position.y = 6.0; - obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; - obj_arr.objects.emplace_back(obj); - - // bus - obj.kinematics.initial_pose_with_covariance.pose.position.x = 4.5; - obj.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; - obj.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(M_PI); - obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::BUS; - obj_arr.objects.emplace_back(obj); - - // Set parameters: enable sidewalk obstacles - behavior_velocity_planner::occlusion_spot_utils::PlannerParam parameters; - parameters.vehicle_info.baselink_to_front = 0.0; - parameters.vehicle_info.vehicle_width = 0.0; - parameters.detection_area_length = 100; - parameters.pedestrian_vel = 1.6; - parameters.lateral_distance_thr = 2.5; - parameters.angle_thr = 2.5; - - auto obj_arr_ptr = - std::make_shared(obj_arr); - auto start_naive = high_resolution_clock::now(); - std::vector - possible_collisions; - createPossibleCollisionBehindParkedVehicle(possible_collisions, path, parameters, 0, obj_arr_ptr); - auto end_naive = high_resolution_clock::now(); - // the possible collision is inserted and - // it's ego distance and obstacle distance is the same as (x,|y|) - ASSERT_EQ(possible_collisions.size(), static_cast(num_car)); - EXPECT_EQ(possible_collisions[0].arc_lane_dist_at_collision.length, 2.5); - EXPECT_EQ(possible_collisions[0].arc_lane_dist_at_collision.distance, 1); - std::cout << " runtime (microsec) " - << duration_cast(end_naive - start_naive).count() << std::endl; + pcs.clear(); } diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index a3921e50b225f..15a0b7a4d5c0a 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -162,12 +162,9 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) this->create_publisher("~/output/occupancy_grid", 1); // Timer - auto timer_callback = std::bind(&CostmapGenerator::onTimer, this); - const auto period = rclcpp::Rate(update_rate_).period(); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(update_rate_).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period_ns, + std::bind(&CostmapGenerator::onTimer, this)); // Initialize initGridmap(); diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index aba12c4718593..478e069539cbe 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -259,11 +259,9 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti // Timer { - auto timer_callback = std::bind(&FreespacePlannerNode::onTimer, this); - const auto period = rclcpp::Rate(node_param_.update_rate).period(); - timer_ = std::make_shared>( - get_clock(), period, std::move(timer_callback), get_node_base_interface()->get_context()); - get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period_ns, + std::bind(&FreespacePlannerNode::onTimer, this)); } } diff --git a/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py index cdddaf483c196..711c35ae0c2d1 100755 --- a/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py +++ b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py @@ -390,7 +390,7 @@ def CalcArcLength(self, traj): p1 = traj.points[i] dx = p1.pose.position.x - p0.pose.position.x dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx ** 2 + dy ** 2) + ds = np.sqrt(dx**2 + dy**2) s_sum += ds s_arr.append(s_sum) return s_arr @@ -408,7 +408,7 @@ def CalcArcLengthPathWLid(self, traj): p1 = traj.points[i].point dx = p1.pose.position.x - p0.pose.position.x dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx ** 2 + dy ** 2) + ds = np.sqrt(dx**2 + dy**2) s_sum += ds s_arr.append(s_sum) return s_arr @@ -426,7 +426,7 @@ def CalcArcLengthPath(self, traj): p1 = traj.points[i] dx = p1.pose.position.x - p0.pose.position.x dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx ** 2 + dy ** 2) + ds = np.sqrt(dx**2 + dy**2) s_sum += ds s_arr.append(s_sum) return s_arr @@ -459,7 +459,7 @@ def CalcAcceleration(self, traj): v = 0.5 * (v1 + v0) dx = p1.pose.position.x - p0.pose.position.x dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx ** 2 + dy ** 2) + ds = np.sqrt(dx**2 + dy**2) dt = ds / max(abs(v), 0.001) a = (v1 - v0) / dt a_arr.append(a) @@ -480,11 +480,11 @@ def CalcJerk(self, traj): dx0 = p1.pose.position.x - p0.pose.position.x dy0 = p1.pose.position.y - p0.pose.position.y - ds0 = np.sqrt(dx0 ** 2 + dy0 ** 2) + ds0 = np.sqrt(dx0**2 + dy0**2) dx1 = p2.pose.position.x - p1.pose.position.x dy1 = p2.pose.position.y - p1.pose.position.y - ds1 = np.sqrt(dx1 ** 2 + dy1 ** 2) + ds1 = np.sqrt(dx1**2 + dy1**2) dt0 = ds0 / max(abs(0.5 * (v1 + v0)), 0.001) dt1 = ds1 / max(abs(0.5 * (v2 + v1)), 0.001) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 9c6edbf1d29a4..5f1c27539bcc1 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -367,7 +367,7 @@ void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::Co // constraints if (!prev_output_.empty()) { // if external velocity limit decreases - if ((external_velocity_limit_ - msg->max_velocity) > eps) { + if (std::fabs((external_velocity_limit_ - msg->max_velocity)) > eps) { if (prev_closest_point_) { const double v0 = prev_closest_point_->longitudinal_velocity_mps; const double a0 = prev_closest_point_->acceleration_mps2; @@ -404,9 +404,6 @@ void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::Co } } } - // if external velocity limit increases - } else if ((msg->max_velocity - external_velocity_limit_) > eps) { - max_velocity_with_deceleration_ = msg->max_velocity; } } diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index ac2caafa06651..95274b6a4829d 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -207,18 +207,35 @@ The above method means that the smaller the lateral deviation of the pointcloud, ```plantuml @startuml -title insertTargetVelocity +title insertTargetVelocity() start -:get obstacle pointcloud in detection area; +:get target vehicle point (*1) in detection area ; -if (estimate velocity of target pointcloud?) then (yes(success to estimate)) -else (no(fail to estimate)) +partition Estimate-Target-Velocity { + +if (Is there a DynamicObject on the target vehicle point?) then (yes) +:use the DynamicObject velocity\nas a target vehicle point velocity (*2); +else (no) + + if (The target vehicle point is found in the previous step?) then (yes) + else (no (estimation failed)) stop -endif + endif + + :estimate the target vehicle point velocity \nby the travel distance from the previous step; + + if (The estimated velocity is valid?) then (yes) + else (no (estimation failed)) + stop + endif + :use the estimated velocity\nas a target vehicle point velocity (*2); -if (the velocity of the pointcloud is fast enough?) then (yes) +endif +} + +if (Is the target vehicle point velocity fast enough?) then (yes) else (no) stop endif @@ -228,21 +245,33 @@ else (no) stop endif -:calculate target velocity to insert; +:calculate target velocity to be inserted in the trajectory; if (the target velocity is fast enough?) then (yes) else (no) stop endif -:embed target velocity; +:insert the target velocity; stop @enduml ``` -This module works only when the obstacle pointcloud is found in the detection area of the `Obstacle stop planner` module. -At first, the velocity of the pointcloud is estimated. The velocity estimation uses the velocity information of dynamic objects, or the distance to the point cloud found in the previous step. +(\*1) The target vehicle point is calculated as a closest obstacle PointCloud from ego along the trajectory. + +(\*2) The sources of velocity estimation can be changed by the following ROS parameters. + +- `adaptive_cruise_control.use_object_to_estimate_vel` +- `adaptive_cruise_control.use_pcl_to_estimate_vel` + +This module works only when the target point is found in the detection area of the `Obstacle stop planner` module. + +The first process of this module is to estimate the velocity of the target vehicle point. The velocity estimation uses the velocity information of dynamic objects or the travel distance of the target vehicle point from the previous step. The dynamic object information is primal, and the travel distance estimation is used as a backup in case of the perception failure. +If the target vehicle point is contained in the bounding box of a dynamic object geometrically, the velocity of the dynamic object is used as the target point velocity. +Otherwise, the target point velocity is calculated by the travel distance of the target point from the previous step; that is `(current_position - previous_position) / dt`. Note that this travel distance based estimation fails when the target point is detected in the first time (it mainly happens in the cut-in situation). To improve the stability of the estimation, the median of the calculation result for several steps is used. + +If the calculated velocity is within the threshold range, it is used as the target point velocity. Only when the estimation is succeeded and the estimated velocity exceeds the value of `obstacle_stop_velocity_thresh_*`, the distance to the pointcloud from self-position is calculated. For prevent chattering in the mode transition, `obstacle_velocity_thresh_to_start_acc` is used for the threshold to start adaptive cruise, and `obstacle_velocity_thresh_to_stop_acc` is used for the threshold to stop adaptive cruise. When the calculated distance value exceeds the emergency distance $d\_{emergency}$ calculated by emergency_stop parameters, target velocity to insert is calculated. diff --git a/planning/planning_error_monitor/src/invalid_trajectory_publisher.cpp b/planning/planning_error_monitor/src/invalid_trajectory_publisher.cpp index 551111a49639d..2931b00939cef 100644 --- a/planning/planning_error_monitor/src/invalid_trajectory_publisher.cpp +++ b/planning/planning_error_monitor/src/invalid_trajectory_publisher.cpp @@ -33,10 +33,8 @@ InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode( traj_pub_ = create_publisher("~/output/trajectory", 1); - auto on_timer_ = std::bind(&InvalidTrajectoryPublisherNode::onTimer, this); - timer_ = std::make_shared>( - this->get_clock(), 100ms, std::move(on_timer_), this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&InvalidTrajectoryPublisherNode::onTimer, this)); } void InvalidTrajectoryPublisherNode::onTimer() diff --git a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp index fa5a0073d9cfd..b686ce70a4b43 100644 --- a/planning/planning_error_monitor/src/planning_error_monitor_node.cpp +++ b/planning/planning_error_monitor/src/planning_error_monitor_node.cpp @@ -50,10 +50,8 @@ PlanningErrorMonitorNode::PlanningErrorMonitorNode(const rclcpp::NodeOptions & n "trajectory_relative_angle_validation", this, &PlanningErrorMonitorNode::onTrajectoryRelativeAngleChecker); - auto on_timer_ = std::bind(&PlanningErrorMonitorNode::onTimer, this); - timer_ = std::make_shared>( - this->get_clock(), 100ms, std::move(on_timer_), this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&PlanningErrorMonitorNode::onTimer, this)); // Parameter error_interval_ = declare_parameter("error_interval", 100.0); diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 10b8648d4e3bc..75c68da8f8f79 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -359,14 +359,10 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti "output/trajectory", rclcpp::QoS{1}); // Timer Callback - auto timer_callback = std::bind(&ScenarioSelectorNode::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / static_cast(update_rate_))); - - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(static_cast(update_rate_)).period(); + + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); // Wait for first tf while (rclcpp::ok()) { diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 950521b92424d..f30af248e4080 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -7,7 +7,8 @@ 1. Correct yaw rate offset by reading the parameter. 2. Correct yaw rate standard deviation by reading the parameter. -Use the value estimated by [deviation_estimator](https://github.com/tier4/calibration_tools/tree/main/localization/deviation_estimation_tools) as the parameters for this node. + + ## Inputs / Outputs diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp index c3bae9c5ad61f..7083ab14e99d4 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -61,12 +61,16 @@ #include // ROS includes +#include "autoware_point_types/types.hpp" + #include +#include #include #include #include #include +#include #include #include @@ -75,12 +79,13 @@ #include #include #include -#include #include #include namespace pointcloud_preprocessor { +using autoware_point_types::PointXYZI; +using point_cloud_msg_wrapper::PointCloud2Modifier; /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single @@ -149,30 +154,18 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node PointCloud2::SharedPtr & out); void publish(); - void removeRADTFields( - const sensor_msgs::msg::PointCloud2 & input_cloud, - sensor_msgs::msg::PointCloud2 & output_cloud); + void convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); void cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, - const std::string & topic_name); + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name); void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); void timer_callback(); void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); }; -struct PointXYZI -{ - PCL_ADD_POINT4D; - float intensity; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; - } // namespace pointcloud_preprocessor -POINT_CLOUD_REGISTER_POINT_STRUCT( - pointcloud_preprocessor::PointXYZI, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)) - #endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index aca9d929a50ec..4b1e3830f5614 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -19,6 +19,7 @@ autoware_auto_vehicle_msgs autoware_point_types + cv_bridge diagnostic_updater image_transport lanelet2_extension diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index 8ce39239de37e..b59d52010f831 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -151,12 +151,11 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Set timer { - auto cb = std::bind(&PointCloudConcatenateDataSynchronizerComponent::timer_callback, this); - auto period = std::chrono::duration_cast( + const auto period_ns = std::chrono::duration_cast( std::chrono::duration(timeout_sec_)); - timer_ = std::make_shared>( - get_clock(), period, std::move(cb), get_node_base_interface()->get_context()); - get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&PointCloudConcatenateDataSynchronizerComponent::timer_callback, this)); } // Diagnostic Updater @@ -297,26 +296,41 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -void PointCloudConcatenateDataSynchronizerComponent::removeRADTFields( - const sensor_msgs::msg::PointCloud2 & input_cloud, sensor_msgs::msg::PointCloud2 & output_cloud) +void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { + output_ptr->header = input_ptr->header; + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + output_modifier.reserve(input_ptr->width); + bool has_intensity = std::any_of( - input_cloud.fields.begin(), input_cloud.fields.end(), + input_ptr->fields.begin(), input_ptr->fields.end(), [](auto & field) { return field.name == "intensity"; }); - if (input_cloud.fields.size() == 3 || (input_cloud.fields.size() == 4 && has_intensity)) { - output_cloud = input_cloud; - } else if (has_intensity) { - pcl::PointCloud tmp_cloud; - pcl::fromROSMsg(input_cloud, tmp_cloud); - pcl::toROSMsg(tmp_cloud, output_cloud); - output_cloud.header = input_cloud.header; + sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); + + if (has_intensity) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + output_modifier.push_back(std::move(point)); + } } else { - pcl::PointCloud tmp_cloud; - pcl::fromROSMsg(input_cloud, tmp_cloud); - pcl::toROSMsg(tmp_cloud, output_cloud); - output_cloud.header = input_cloud.header; + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = 0.0f; + output_modifier.push_back(std::move(point)); + } } } @@ -337,14 +351,12 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new } void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name) { std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2 xyz_cloud; - removeRADTFields(*input_ptr, xyz_cloud); - sensor_msgs::msg::PointCloud2::ConstSharedPtr xyz_input_ptr( - new sensor_msgs::msg::PointCloud2(xyz_cloud)); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZICloud(input_ptr, xyzi_input_ptr); const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( @@ -352,7 +364,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyz_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -365,7 +377,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyz_input_ptr; + cloud_stdmap_[topic_name] = xyzi_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_), diff --git a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp index fe1df627f143d..9b3f01fa4b267 100644 --- a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp +++ b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp @@ -28,12 +28,9 @@ class EmptyObjectsPublisher : public rclcpp::Node this->create_publisher( "~/output/objects", 1); - auto timer_callback = std::bind(&EmptyObjectsPublisher::timerCallback, this); - const auto period = std::chrono::milliseconds(100); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&EmptyObjectsPublisher::timerCallback, this)); } private: diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 3104369549a8a..da5939bccea7d 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -46,12 +46,9 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() "input/object", 100, std::bind(&DummyPerceptionPublisherNode::objectCallback, this, std::placeholders::_1)); - auto timer_callback = std::bind(&DummyPerceptionPublisherNode::timerCallback, this); - auto period = std::chrono::milliseconds(100); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&DummyPerceptionPublisherNode::timerCallback, this)); } void DummyPerceptionPublisherNode::timerCallback() diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp index 74e18f70ca076..3fd02bfa8a682 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp @@ -460,12 +460,7 @@ AutowareStateMonitorNode::AutowareStateMonitorNode() std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Timer - auto timer_callback = std::bind(&AutowareStateMonitorNode::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / update_rate_)); - - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, callback_group_subscribers_); + const auto period_ns = rclcpp::Rate(update_rate_).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&AutowareStateMonitorNode::onTimer, this)); } diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp b/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp index e128411b6283b..515581fa27a74 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp +++ b/system/dummy_diag_publisher/src/dummy_diag_publisher_node/dummy_diag_publisher_node.cpp @@ -95,12 +95,7 @@ DummyDiagPublisherNode::DummyDiagPublisherNode() : Node("dummy_diag_publisher") updater_.add(diag_config_.name, this, &DummyDiagPublisherNode::produceDiagnostics); // Timer - auto timer_callback = std::bind(&DummyDiagPublisherNode::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1 / update_rate_)); - - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(update_rate_).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&DummyDiagPublisherNode::onTimer, this)); } diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index 7b117eff30439..50a5ed851c29f 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -67,13 +67,9 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") new autoware_auto_control_msgs::msg::AckermannControlCommand); // Timer - auto timer_callback = std::bind(&EmergencyHandler::onTimer, this); const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); - - timer_ = std::make_shared>( - this->get_clock(), update_period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, std::bind(&EmergencyHandler::onTimer, this)); } void EmergencyHandler::onHazardStatusStamped( diff --git a/system/system_error_monitor/README.md b/system/system_error_monitor/README.md index 1fcc74475b19b..d1bfbdc95488d 100644 --- a/system/system_error_monitor/README.md +++ b/system/system_error_monitor/README.md @@ -90,12 +90,12 @@ endif ### Input -| Name | Type | Description | -| ---------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](https://github.com/tier4/autoware.iv/tree/main/system/system_error_monitor/config/diagnostic_aggregator) is used to | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | -| `/vehicle/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | +| Name | Type | Description | +| ---------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | +| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | +| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | +| `/vehicle/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | ### Output diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 2fc2133af1b33..12fdafce41708 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -243,14 +243,9 @@ AutowareErrorMonitor::AutowareErrorMonitor() // Timer initialized_time_ = this->now(); - auto timer_callback = std::bind(&AutowareErrorMonitor::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / params_.update_rate)); - - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(params_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&AutowareErrorMonitor::onTimer, this)); } void AutowareErrorMonitor::loadRequiredModules(const std::string & key) diff --git a/system/topic_state_monitor/src/topic_state_monitor_core.cpp b/system/topic_state_monitor/src/topic_state_monitor_core.cpp index ccc7d7bd6b79f..739826c3f4b04 100644 --- a/system/topic_state_monitor/src/topic_state_monitor_core.cpp +++ b/system/topic_state_monitor/src/topic_state_monitor_core.cpp @@ -79,14 +79,9 @@ TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_op updater_.add(param_.diag_name, this, &TopicStateMonitorNode::checkTopicStatus); // Timer - auto timer_callback = std::bind(&TopicStateMonitorNode::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / node_param_.update_rate)); - - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&TopicStateMonitorNode::onTimer, this)); } rcl_interfaces::msg::SetParametersResult TopicStateMonitorNode::onParameter( diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 1b9c8ee9c275d..60873f674fbd4 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -201,25 +201,19 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) { - auto timer_output_csv_callback = - std::bind(&AccelBrakeMapCalibrator::timerCallbackOutputCSV, this); const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_output_csv_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_output_csv_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_output_csv_, nullptr); + timer_output_csv_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&AccelBrakeMapCalibrator::timerCallbackOutputCSV, this)); } void AccelBrakeMapCalibrator::initTimer(double period_s) { - auto timer_callback = std::bind(&AccelBrakeMapCalibrator::timerCallback, this); const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = std::make_shared>( - this->get_clock(), period_ns, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&AccelBrakeMapCalibrator::timerCallback, this)); } bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) diff --git a/vehicle/external_cmd_converter/src/node.cpp b/vehicle/external_cmd_converter/src/node.cpp index c9dac8482bfde..9be35847d0606 100644 --- a/vehicle/external_cmd_converter/src/node.cpp +++ b/vehicle/external_cmd_converter/src/node.cpp @@ -50,13 +50,9 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n control_command_timeout_ = declare_parameter("control_command_timeout", 1.0); emergency_stop_timeout_ = declare_parameter("emergency_stop_timeout", 3.0); - auto timer_callback = std::bind(&ExternalCmdConverterNode::onTimer, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / timer_rate)); - rate_check_timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(rate_check_timer_, nullptr); + const auto period_ns = rclcpp::Rate(timer_rate).period(); + rate_check_timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ExternalCmdConverterNode::onTimer, this)); // Parameter for accel/brake map const std::string csv_path_accel_map = declare_parameter("csv_path_accel_map"); diff --git a/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp b/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp index 1b7f9880ed94b..f9fb82e3a4dc8 100644 --- a/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp +++ b/vehicle/pacmod_interface/include/pacmod_interface/pacmod_interface.hpp @@ -125,15 +125,16 @@ class PacmodInterface : public rclcpp::Node bool is_pacmod_enabled_ = false; bool is_clear_override_needed_ = false; bool prev_override_ = false; - double loop_rate_; // [Hz] - double tire_radius_; // [m] - double wheel_base_; // [m] - double steering_offset_; // [rad] def: measured = truth + offset - double vgr_coef_a_; // variable gear ratio coeffs - double vgr_coef_b_; // variable gear ratio coeffs - double vgr_coef_c_; // variable gear ratio coeffs - double accel_pedal_offset_; // offset of accel pedal value - double brake_pedal_offset_; // offset of brake pedal value + double loop_rate_; // [Hz] + double tire_radius_; // [m] + double wheel_base_; // [m] + double steering_offset_; // [rad] def: measured = truth + offset + double vgr_coef_a_; // variable gear ratio coeffs + double vgr_coef_b_; // variable gear ratio coeffs + double vgr_coef_c_; // variable gear ratio coeffs + double accel_pedal_offset_; // offset of accel pedal value + double brake_pedal_offset_; // offset of brake pedal value + double tire_radius_scale_factor_; // scale factor of tire radius double emergency_brake_; // brake command when emergency [m/s^2] double max_throttle_; // max throttle [0~1] diff --git a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp index 75121118c33d4..d833cc97fbf75 100644 --- a/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp +++ b/vehicle/pacmod_interface/src/pacmod_interface/pacmod_interface.cpp @@ -44,6 +44,7 @@ PacmodInterface::PacmodInterface() vgr_coef_c_ = declare_parameter("vgr_coef_c", 0.042); accel_pedal_offset_ = declare_parameter("accel_pedal_offset", 0.0); brake_pedal_offset_ = declare_parameter("brake_pedal_offset", 0.0); + tire_radius_scale_factor_ = declare_parameter("tire_radius_scale_factor", 1.0); /* parameters for limitter */ max_throttle_ = declare_parameter("max_throttle", 0.2); @@ -150,13 +151,9 @@ PacmodInterface::PacmodInterface() create_publisher("/vehicle/status/steering_wheel_status", 1); // Timer - auto timer_callback = std::bind(&PacmodInterface::publishCommands, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / loop_rate_)); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + const auto period_ns = rclcpp::Rate(loop_rate_).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PacmodInterface::publishCommands, this)); } void PacmodInterface::callbackActuationCmd(const ActuationCommandStamped::ConstSharedPtr msg) @@ -525,7 +522,7 @@ double PacmodInterface::calculateVehicleVelocity( const double sign = (shift_rpt.output == pacmod3_msgs::msg::SystemRptInt::SHIFT_REVERSE) ? -1 : 1; const double vel = (wheel_speed_rpt.rear_left_wheel_speed + wheel_speed_rpt.rear_right_wheel_speed) * 0.5 * - tire_radius_; + tire_radius_ * tire_radius_scale_factor_; return sign * vel; }