diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index be25169b1604e..acf3c48b70e93 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -83,6 +83,8 @@ - source: .github/workflows/check-build-depends.yaml pre-commands: | sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {source} + - source: .github/workflows/clang-tidy-pr-comments.yaml + - source: .github/workflows/clang-tidy-pr-comments-manually.yaml - source: .github/workflows/update-codeowners-from-packages.yaml - source: codecov.yaml diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index fb08c73f5790b..43bb8f7bbfea6 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -80,7 +80,7 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v32 + uses: tj-actions/changed-files@v34 with: files: | **/*.cpp diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml new file mode 100644 index 0000000000000..f593f034cdf6a --- /dev/null +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -0,0 +1,62 @@ +name: clang-tidy-pr-comments-manually + +on: + workflow_dispatch: + inputs: + workflow_run_id_or_url: + description: The target workflow run ID or URL of the build-and-test-differential workflow + required: true +jobs: + clang-tidy-pr-comments-manually: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v3 + + - name: Download analysis results + run: | + workflow_run_id=$(echo "${{ inputs.workflow_run_id_or_url }}" | sed -e "s|.*runs/||" -e "s|/jobs.*||") + gh run download "$workflow_run_id" -D /tmp || true + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + + - name: Check if the fixes.yaml file exists + id: check-fixes-yaml-existence + uses: autowarefoundation/autoware-github-actions/check-file-existence@v1 + with: + files: /tmp/clang-tidy-result/fixes.yaml + + - name: Set variables + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} + id: set-variables + run: | + echo ::set-output name=pr-id::"$(cat /tmp/clang-tidy-result/pr-id.txt)" + echo ::set-output name=pr-head-repo::"$(cat /tmp/clang-tidy-result/pr-head-repo.txt)" + echo ::set-output name=pr-head-ref::"$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" + + - name: Check out PR head + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} + uses: actions/checkout@v3 + with: + repository: ${{ steps.set-variables.outputs.pr-head-repo }} + ref: ${{ steps.set-variables.outputs.pr-head-ref }} + persist-credentials: false + + - name: Replace paths in fixes.yaml + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} + run: | + sed -i -e "s|/__w/|/home/runner/work/|g" /tmp/clang-tidy-result/fixes.yaml + cat /tmp/clang-tidy-result/fixes.yaml + + - name: Copy fixes.yaml to access from Docker Container Action + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} + run: | + cp /tmp/clang-tidy-result/fixes.yaml fixes.yaml + + - name: Run clang-tidy-pr-comments action + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} + uses: platisd/clang-tidy-pr-comments@v1 + with: + github_token: ${{ secrets.GITHUB_TOKEN }} + clang_tidy_fixes: fixes.yaml + pull_request_id: ${{ steps.set-variables.outputs.pr-id }} diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index 228121062fbcd..75849bd960170 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -9,7 +9,7 @@ on: jobs: clang-tidy-pr-comments: - if: ${{ github.event.workflow_run.event == 'pull_request' && github.event.workflow_run.conclusion == 'success' }} + if: ${{ github.event.workflow_run.event == 'pull_request' && contains(fromJson('["success", "failure"]'), github.event.workflow_run.conclusion) }} runs-on: ubuntu-latest steps: - name: Check out repository @@ -17,11 +17,18 @@ jobs: - name: Download analysis results run: | - gh run download ${{ github.event.workflow_run.id }} -D /tmp + gh run download ${{ github.event.workflow_run.id }} -D /tmp || true env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + - name: Check if the fixes.yaml file exists + id: check-fixes-yaml-existence + uses: autowarefoundation/autoware-github-actions/check-file-existence@v1 + with: + files: /tmp/clang-tidy-result/fixes.yaml + - name: Set variables + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} id: set-variables run: | echo ::set-output name=pr-id::"$(cat /tmp/clang-tidy-result/pr-id.txt)" @@ -29,6 +36,7 @@ jobs: echo ::set-output name=pr-head-ref::"$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" - name: Check out PR head + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} uses: actions/checkout@v3 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} @@ -36,15 +44,18 @@ jobs: persist-credentials: false - name: Replace paths in fixes.yaml + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} run: | sed -i -e "s|/__w/|/home/runner/work/|g" /tmp/clang-tidy-result/fixes.yaml cat /tmp/clang-tidy-result/fixes.yaml - name: Copy fixes.yaml to access from Docker Container Action + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} run: | cp /tmp/clang-tidy-result/fixes.yaml fixes.yaml - name: Run clang-tidy-pr-comments action + if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} uses: platisd/clang-tidy-pr-comments@v1 with: github_token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.markdown-link-check.json b/.markdown-link-check.json index dec3db1ad1c8d..c71a3e4253687 100644 --- a/.markdown-link-check.json +++ b/.markdown-link-check.json @@ -4,6 +4,9 @@ { "pattern": "^http://localhost" }, + { + "pattern": "^http://127\\.0\\.0\\.1" + }, { "pattern": "^https://github.com/.*/discussions/new" } diff --git a/.markdownlint.yaml b/.markdownlint.yaml index df1f518dc0d48..babaaa1f1586d 100644 --- a/.markdownlint.yaml +++ b/.markdownlint.yaml @@ -3,6 +3,8 @@ default: true MD013: false MD024: siblings_only: true +MD029: + style: ordered MD033: false MD041: false MD046: false diff --git a/common/interpolation/include/interpolation/interpolation_utils.hpp b/common/interpolation/include/interpolation/interpolation_utils.hpp index ed381a3108410..9c0372f788ecb 100644 --- a/common/interpolation/include/interpolation/interpolation_utils.hpp +++ b/common/interpolation/include/interpolation/interpolation_utils.hpp @@ -15,6 +15,7 @@ #ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_ #define INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#include #include #include #include @@ -51,7 +52,7 @@ inline bool isNotDecreasing(const std::vector & x) return true; } -inline void validateKeys( +inline std::vector validateKeys( const std::vector & base_keys, const std::vector & query_keys) { // when vectors are empty @@ -71,9 +72,20 @@ inline void validateKeys( } // when query_keys is out of base_keys (This function does not allow exterior division.) - if (query_keys.front() < base_keys.front() || base_keys.back() < query_keys.back()) { + constexpr double epsilon = 1e-3; + if ( + query_keys.front() < base_keys.front() - epsilon || + base_keys.back() + epsilon < query_keys.back()) { throw std::invalid_argument("query_keys is out of base_keys"); } + + // NOTE: Due to calculation error of double, a query key may be slightly out of base keys. + // Therefore, query keys are cropped here. + auto validated_query_keys = query_keys; + validated_query_keys.front() = std::max(validated_query_keys.front(), base_keys.front()); + validated_query_keys.back() = std::min(validated_query_keys.back(), base_keys.back()); + + return validated_query_keys; } template diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/interpolation/include/interpolation/zero_order_hold.hpp index e48da814c5740..1142cb544c174 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/interpolation/include/interpolation/zero_order_hold.hpp @@ -27,20 +27,20 @@ std::vector zero_order_hold( const std::vector & query_keys, const double overlap_threshold = 1e-3) { // throw exception for invalid arguments - interpolation_utils::validateKeys(base_keys, query_keys); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); interpolation_utils::validateKeysAndValues(base_keys, base_values); std::vector query_values; size_t closest_segment_idx = 0; - for (size_t i = 0; i < query_keys.size(); ++i) { + for (size_t i = 0; i < validated_query_keys.size(); ++i) { // Check if query_key is closes to the terminal point of the base keys - if (base_keys.back() - overlap_threshold < query_keys.at(i)) { + if (base_keys.back() - overlap_threshold < validated_query_keys.at(i)) { closest_segment_idx = base_keys.size() - 1; } else { for (size_t j = closest_segment_idx; j < base_keys.size() - 1; ++j) { if ( - base_keys.at(j) - overlap_threshold < query_keys.at(i) && - query_keys.at(i) < base_keys.at(j + 1)) { + base_keys.at(j) - overlap_threshold < validated_query_keys.at(i) && + validated_query_keys.at(i) < base_keys.at(j + 1)) { // find closest segment in base keys closest_segment_idx = j; } diff --git a/common/interpolation/src/linear_interpolation.cpp b/common/interpolation/src/linear_interpolation.cpp index 32d3654dbbdd8..f74d085dfee9e 100644 --- a/common/interpolation/src/linear_interpolation.cpp +++ b/common/interpolation/src/linear_interpolation.cpp @@ -28,13 +28,13 @@ std::vector lerp( const std::vector & query_keys) { // throw exception for invalid arguments - interpolation_utils::validateKeys(base_keys, query_keys); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); interpolation_utils::validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; size_t key_index = 0; - for (const auto query_key : query_keys) { + for (const auto query_key : validated_query_keys) { while (base_keys.at(key_index + 1) < query_key) { ++key_index; } diff --git a/common/interpolation/src/spherical_linear_interpolation.cpp b/common/interpolation/src/spherical_linear_interpolation.cpp index 014e9011e2a61..c3595d212f349 100644 --- a/common/interpolation/src/spherical_linear_interpolation.cpp +++ b/common/interpolation/src/spherical_linear_interpolation.cpp @@ -34,13 +34,13 @@ std::vector slerp( const std::vector & query_keys) { // throw exception for invalid arguments - interpolation_utils::validateKeys(base_keys, query_keys); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); interpolation_utils::validateKeysAndValues(base_keys, base_values); // calculate linear interpolation std::vector query_values; size_t key_index = 0; - for (const auto query_key : query_keys) { + for (const auto query_key : validated_query_keys) { while (base_keys.at(key_index + 1) < query_key) { ++key_index; } diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index cf00452f1d850..bd92af1007b50 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -222,7 +222,7 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - interpolation_utils::validateKeys(base_keys_, query_keys); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); const auto & a = multi_spline_coef_.a; const auto & b = multi_spline_coef_.b; @@ -231,7 +231,7 @@ std::vector SplineInterpolation::getSplineInterpolatedValues( std::vector res; size_t j = 0; - for (const auto & query_key : query_keys) { + for (const auto & query_key : validated_query_keys) { while (base_keys_.at(j + 1) < query_key) { ++j; } @@ -247,7 +247,7 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( const std::vector & query_keys) const { // throw exceptions for invalid arguments - interpolation_utils::validateKeys(base_keys_, query_keys); + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); const auto & a = multi_spline_coef_.a; const auto & b = multi_spline_coef_.b; @@ -255,7 +255,7 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( std::vector res; size_t j = 0; - for (const auto & query_key : query_keys) { + for (const auto & query_key : validated_query_keys) { while (base_keys_.at(j + 1) < query_key) { ++j; } diff --git a/common/interpolation/test/src/test_interpolation_utils.cpp b/common/interpolation/test/src/test_interpolation_utils.cpp index 3eb40fd439c56..8b3a3b9faa0c6 100644 --- a/common/interpolation/test/src/test_interpolation_utils.cpp +++ b/common/interpolation/test/src/test_interpolation_utils.cpp @@ -95,6 +95,23 @@ TEST(interpolation_utils, validateKeys) const std::vector back_out_query_keys{0.0, 1.0, 2.0, 4.0}; EXPECT_THROW(validateKeys(base_keys, back_out_query_keys), std::invalid_argument); + + { // validated key check in normal case + const std::vector normal_query_keys{0.5, 1.5, 3.0}; + const auto validated_query_keys = validateKeys(base_keys, normal_query_keys); + for (size_t i = 0; i < normal_query_keys.size(); ++i) { + EXPECT_EQ(normal_query_keys.at(i), validated_query_keys.at(i)); + } + } + + { // validated key check in case slightly out of range + constexpr double slightly_out_of_range_epsilon = 1e-6; + const std::vector slightly_out_of_range__query_keys{ + 0.0 - slightly_out_of_range_epsilon, 3.0 + slightly_out_of_range_epsilon}; + const auto validated_query_keys = validateKeys(base_keys, slightly_out_of_range__query_keys); + EXPECT_NEAR(validated_query_keys.at(0), 0.0, 1e-10); + EXPECT_NEAR(validated_query_keys.at(1), 3.0, 1e-10); + } } TEST(interpolation_utils, validateKeysAndValues) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 5cc81aa04b750..2a119fa02177d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1066,7 +1066,7 @@ inline boost::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - points_with_twist.at(i).longitudinal_velocity_mps = 0.0; + tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; @@ -1099,7 +1099,7 @@ inline boost::optional insertStopPoint( } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { - points_with_twist.at(i).longitudinal_velocity_mps = 0.0; + tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return stop_idx; diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp index 06dea47300711..24d737a154e86 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -138,6 +138,12 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, const std::vector & u); + // Setter functions for warm start + bool setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables); + bool setPrimalVariables(const std::vector & primal_variables); + bool setDualVariables(const std::vector & dual_variables); + // Updates problem parameters while keeping solution in memory. // // Args: @@ -161,6 +167,10 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface void updateRhoInterval(const int rho_interval); void updateRho(const double rho); void updateAlpha(const double alpha); + void updateScaling(const int scaling); + void updatePolish(const bool polish); + void updatePolishRefinementIteration(const int polish_refine_iter); + void updateCheckTermination(const int check_termination); /// \brief Get the number of iteration taken to solve the problem inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp index b134453862701..085faf8f14bd6 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -208,6 +208,78 @@ void OSQPInterface::updateAlpha(const double alpha) } } +void OSQPInterface::updateScaling(const int scaling) { m_settings->scaling = scaling; } + +void OSQPInterface::updatePolish(const bool polish) +{ + m_settings->polish = polish; + if (m_work_initialized) { + osqp_update_polish(m_work.get(), polish); + } +} + +void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) +{ + if (polish_refine_iter < 0) { + std::cerr << "Polish refinement iterations must be positive" << std::endl; + return; + } + + m_settings->polish_refine_iter = polish_refine_iter; + if (m_work_initialized) { + osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + } +} + +void OSQPInterface::updateCheckTermination(const int check_termination) +{ + if (check_termination < 0) { + std::cerr << "Check termination must be positive" << std::endl; + return; + } + + m_settings->check_termination = check_termination; + if (m_work_initialized) { + osqp_update_check_termination(m_work.get(), check_termination); + } +} + +bool OSQPInterface::setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables) +{ + return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); +} + +bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + if (result != 0) { + std::cerr << "Failed to set primal variables for warm start" << std::endl; + return false; + } + + return true; +} + +bool OSQPInterface::setDualVariables(const std::vector & dual_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + if (result != 0) { + std::cerr << "Failed to set dual variables for warm start" << std::endl; + return false; + } + + return true; +} + int64_t OSQPInterface::initializeProblem( const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, const std::vector & l, const std::vector & u) diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index 5f927645d9147..6ef6d131a6cbe 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -131,5 +131,36 @@ TEST(TestOsqpInterface, BasicQp) result = osqp.optimize(); check_result(result); } + + // add warm startup + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + osqp.optimize(); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + osqp.initializeProblem(P_csc, A_csc, q, l, u); + result = osqp.optimize(); + check_result(result); + + osqp.updateCheckTermination(1); + const auto primal_val = std::get<0>(result); + const auto dual_val = std::get<1>(result); + for (size_t i = 0; i < primal_val.size(); ++i) { + std::cerr << primal_val.at(i) << std::endl; + } + osqp.setWarmStart(primal_val, dual_val); + result = osqp.optimize(); + check_result(result); + EXPECT_EQ(osqp.getTakenIter(), 1); + } } } // namespace diff --git a/common/perception_utils/include/perception_utils/object_classification.hpp b/common/perception_utils/include/perception_utils/object_classification.hpp index dba9236f2c3c0..3c38c1f16b21b 100644 --- a/common/perception_utils/include/perception_utils/object_classification.hpp +++ b/common/perception_utils/include/perception_utils/object_classification.hpp @@ -17,6 +17,7 @@ #include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include #include namespace perception_utils @@ -92,6 +93,82 @@ inline bool isLargeVehicle(const std::vector & object_clas auto highest_prob_label = getHighestProbLabel(object_classifications); return isLargeVehicle(highest_prob_label); } + +inline uint8_t toLabel(const std::string & class_name) +{ + if (class_name == "UNKNOWN") { + return ObjectClassification::UNKNOWN; + } else if (class_name == "CAR") { + return ObjectClassification::CAR; + } else if (class_name == "TRUCK") { + return ObjectClassification::TRUCK; + } else if (class_name == "BUS") { + return ObjectClassification::BUS; + } else if (class_name == "TRAILER") { + return ObjectClassification::TRAILER; + } else if (class_name == "MOTORCYCLE") { + return ObjectClassification::MOTORCYCLE; + } else if (class_name == "BICYCLE") { + return ObjectClassification::BICYCLE; + } else if (class_name == "PEDESTRIAN") { + return ObjectClassification::PEDESTRIAN; + } else { + throw std::runtime_error("Invalid Classification label."); + } +} + +inline ObjectClassification toObjectClassification( + const std::string & class_name, float probability) +{ + ObjectClassification classification; + classification.label = toLabel(class_name); + classification.probability = probability; + return classification; +} + +inline std::vector toObjectClassifications( + const std::string & class_name, float probability) +{ + std::vector classifications; + classifications.push_back(toObjectClassification(class_name, probability)); + return classifications; +} + +inline std::string convertLabelToString(const uint8_t label) +{ + if (label == ObjectClassification::UNKNOWN) { + return "UNKNOWN"; + } else if (label == ObjectClassification::CAR) { + return "CAR"; + } else if (label == ObjectClassification::TRUCK) { + return "TRUCK"; + } else if (label == ObjectClassification::BUS) { + return "BUS"; + } else if (label == ObjectClassification::TRAILER) { + return "TRAILER"; + } else if (label == ObjectClassification::MOTORCYCLE) { + return "MOTORCYCLE"; + } else if (label == ObjectClassification::BICYCLE) { + return "BICYCLE"; + } else if (label == ObjectClassification::PEDESTRIAN) { + return "PEDESTRIAN"; + } else { + return "UNKNOWN"; + } +} + +inline std::string convertLabelToString(const ObjectClassification object_classification) +{ + return convertLabelToString(object_classification.label); +} + +inline std::string convertLabelToString( + const std::vector object_classifications) +{ + auto highest_prob_label = getHighestProbLabel(object_classifications); + return convertLabelToString(highest_prob_label); +} + } // namespace perception_utils #endif // PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_ diff --git a/common/perception_utils/test/src/test_object_classification.cpp b/common/perception_utils/test/src/test_object_classification.cpp index 9758266642255..be389634ef5a2 100644 --- a/common/perception_utils/test/src/test_object_classification.cpp +++ b/common/perception_utils/test/src/test_object_classification.cpp @@ -98,3 +98,72 @@ TEST(object_classification, test_getHighestProbClassification) EXPECT_NEAR(classification.probability, 0.8, epsilon); } } + +TEST(object_classification, test_fromString) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using perception_utils::toLabel; + using perception_utils::toObjectClassification; + using perception_utils::toObjectClassifications; + + // toLabel + { + EXPECT_EQ(toLabel("UNKNOWN"), ObjectClassification::UNKNOWN); + EXPECT_EQ(toLabel("CAR"), ObjectClassification::CAR); + EXPECT_EQ(toLabel("TRUCK"), ObjectClassification::TRUCK); + EXPECT_EQ(toLabel("BUS"), ObjectClassification::BUS); + EXPECT_EQ(toLabel("TRAILER"), ObjectClassification::TRAILER); + EXPECT_EQ(toLabel("MOTORCYCLE"), ObjectClassification::MOTORCYCLE); + EXPECT_EQ(toLabel("BICYCLE"), ObjectClassification::BICYCLE); + EXPECT_EQ(toLabel("PEDESTRIAN"), ObjectClassification::PEDESTRIAN); + EXPECT_THROW(toLabel(""), std::runtime_error); + } + + // Classification + { + auto classification = toObjectClassification("CAR", 0.7); + EXPECT_EQ(classification.label, ObjectClassification::CAR); + EXPECT_NEAR(classification.probability, 0.7, epsilon); + } + // Classifications + { + auto classifications = toObjectClassifications("CAR", 0.7); + EXPECT_EQ(classifications.at(0).label, ObjectClassification::CAR); + EXPECT_NEAR(classifications.at(0).probability, 0.7, epsilon); + } +} + +TEST(object_classification, test_convertLabelToString) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using perception_utils::convertLabelToString; + + // from label + { + EXPECT_EQ(convertLabelToString(ObjectClassification::UNKNOWN), "UNKNOWN"); + EXPECT_EQ(convertLabelToString(ObjectClassification::CAR), "CAR"); + EXPECT_EQ(convertLabelToString(ObjectClassification::TRUCK), "TRUCK"); + EXPECT_EQ(convertLabelToString(ObjectClassification::BUS), "BUS"); + EXPECT_EQ(convertLabelToString(ObjectClassification::TRAILER), "TRAILER"); + EXPECT_EQ(convertLabelToString(ObjectClassification::MOTORCYCLE), "MOTORCYCLE"); + EXPECT_EQ(convertLabelToString(ObjectClassification::BICYCLE), "BICYCLE"); + EXPECT_EQ(convertLabelToString(ObjectClassification::PEDESTRIAN), "PEDESTRIAN"); + } + + // from ObjectClassification + { + auto classification = createObjectClassification(ObjectClassification::CAR, 0.8); + + EXPECT_EQ(convertLabelToString(classification), "CAR"); + } + + // from ObjectClassifications + { + std::vector classifications; + classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); + classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + + EXPECT_EQ(convertLabelToString(classifications), "TRUCK"); + } +} diff --git a/common/signal_processing/CMakeLists.txt b/common/signal_processing/CMakeLists.txt index cca36dcac0341..89f14d4339223 100644 --- a/common/signal_processing/CMakeLists.txt +++ b/common/signal_processing/CMakeLists.txt @@ -4,19 +4,21 @@ project(signal_processing) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(lowpass_filter_1d SHARED +ament_auto_add_library(lowpass_filters SHARED src/lowpass_filter_1d.cpp src/lowpass_filter.cpp -) + src/butterworth.cpp) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_signal_processing test/src/lowpass_filter_1d_test.cpp test/src/lowpass_filter_test.cpp - ) + test/src/butterworth_filter_test.cpp) + + target_include_directories(test_signal_processing PUBLIC test/include) target_link_libraries(test_signal_processing - lowpass_filter_1d - ) + lowpass_filters) + endif() ament_auto_package() diff --git a/common/signal_processing/README.md b/common/signal_processing/README.md index cb566061a87ff..5f7aaa978ed6d 100644 --- a/common/signal_processing/README.md +++ b/common/signal_processing/README.md @@ -1,4 +1,10 @@ -# signal_processing +# Signal Processing Methods + +In this package, we present signal processing related methods for the Autoware applications. The following +functionalities are available in the current version. + +- an 1-D Low-pass filter, +- [Butterworth low-pass filter tools.](documentation/ButterworthFilter.md) low-pass filter currently supports only the 1-D low pass filtering. diff --git a/common/signal_processing/documentation/ButterworthFilter.md b/common/signal_processing/documentation/ButterworthFilter.md new file mode 100644 index 0000000000000..e29ce1f4cfb69 --- /dev/null +++ b/common/signal_processing/documentation/ButterworthFilter.md @@ -0,0 +1,124 @@ +### Butterworth Low-pass Filter Design Tool Class + +This Butterworth low-pass filter design tool can be used to design a Butterworth filter in continuous and discrete-time +from the given specifications of the filter performance. The Butterworth filter is a class implementation. A default +constructor creates the object without any argument. + +The filter can be prepared in three ways. If the filter specifications are known, such as the pass-band, and stop-band +frequencies (Wp and Ws) together with the pass-band and stop-band ripple magnitudes (Ap and As), one can call the +filter's buttord method with these arguments to obtain the recommended filter order (N) and cut-off frequency +(Wc_rad_sec [rad/s]). + +![img.png](img.png) +Figure 1. Butterworth Low-pass filter specification from [1]. + +An example call is demonstrated below; + + ButterworthFilter bf(); + + Wp = 2.0; // pass-band frequency [rad/sec] + Ws = 3.0; // stop-band frequency [rad/sec] + Ap = 6.0; // pass-band ripple mag or loss [dB] + As = 20.0; // stop band ripple attenuation [dB] + + // Computing filter coefficients from the specs + bf.Buttord(Wp, Ws, Ap, As); + + // Get the computed order and Cut-Off frequency + sOrderCutOff NWc = bf.getOrderCutOff();] + + cout << " The computed order is ;" << NWc.N << endl; + cout << " The computed cut-off frequency is ;" << NWc.Wc_rad_sec << endl; + +The filter order and cut-off frequency can be obtained in a struct using bf.getOrderCutoff() method. These specs can be +printed on the screen by calling PrintFilterSpecs() method. If the user would like to define the order and cut-off +frequency manually, the setter methods for these variables can be called to set the filter order (N) and the desired +cut-off frequency (Wc_rad_sec [rad/sec]) for the filter. + +#### Obtaining Filter Transfer Functions + +The discrete transfer function of the filter requires the roots and gain of the continuous-time transfer function. +Therefore, it is a must to call the first computeContinuousTimeTF() to create the continuous-time transfer function +of the filter using; + + bf.computeContinuousTimeTF(); + +The computed continuous-time transfer function roots can be printed on the screen using the methods; + + bf.PrintFilter_ContinuousTimeRoots(); + bf.PrintContinuousTimeTF(); + +The resulting screen output for a 5th order filter is demonstrated below. + + Roots of Continuous Time Filter Transfer Function Denominator are : + -0.585518 + j 1.80204 + -1.53291 + j 1.11372 + -1.89478 + j 2.32043e-16 + -1.53291 + j -1.11372 + -0.585518 + j -1.80204 + + + The Continuous-Time Transfer Function of the Filter is ; + + 24.422 + ------------------------------------------------------------------------------- + 1.000 *s[5] + 6.132 *s[4] + 18.798 *s[3] + 35.619 *s[2] + 41.711 *s[1] + 24.422 + +#### Discrete Time Transfer Function (Difference Equations) + +The digital filter equivalent of the continuous-time definitions is produced by using the bi-linear transformation. +When creating the discrete-time function of the ButterworthFilter object, its Numerator (Bn) and Denominator (An +) coefficients are stored in a vector of filter order size N. + +The discrete transfer function method is called using ; + + bf.computeDiscreteTimeTF(); + bf.PrintDiscreteTimeTF(); + +The results are printed on the screen like; +The Discrete-Time Transfer Function of the Filter is ; + + 0.191 *z[-5] + 0.956 *z[-4] + 1.913 *z[-3] + 1.913 *z[-2] + 0.956 *z[-1] + 0.191 + -------------------------------------------------------------------------------- + 1.000 *z[-5] + 1.885 *z[-4] + 1.888 *z[-3] + 1.014 *z[-2] + 0.298 *z[-1] + 0.037 + +and the associated difference coefficients An and Bn by withing a struct ; + + sDifferenceAnBn AnBn = bf.getAnBn(); + +The difference coefficients appear in the filtering equation in the form of. + + An * Y_filtered = Bn * Y_unfiltered + +To filter a signal given in a vector form ; + +#### Calling Filter by a specified cut-off and sampling frequencies [in Hz] + +The Butterworth filter can also be created by defining the desired order (N), a cut-off frequency (fc in [Hz]), and a +sampling frequency (fs in [Hz]). In this method, the cut-off frequency is pre-warped with respect to the sampling +frequency [1, 2] to match the continuous and digital filter frequencies. + +The filter is prepared by the following calling options; + + // 3rd METHOD defining a sampling frequency together with the cut-off fc, fs + bf.setOrder(2); + bf.setCutOffFrequency(10, 100); + +At this step, we define a boolean variable whether to use the pre-warping option or not. + + // Compute Continuous Time TF + bool use_sampling_frequency = true; + bf.computeContinuousTimeTF(use_sampling_frequency); + bf.PrintFilter_ContinuousTimeRoots(); + bf.PrintContinuousTimeTF(); + + // Compute Discrete Time TF + bf.computeDiscreteTimeTF(use_sampling_frequency); + bf.PrintDiscreteTimeTF(); + +**References:** + +1. Manolakis, Dimitris G., and Vinay K. Ingle. Applied digital signal processing: theory and practice. Cambridge + University Press, 2011. + +2. diff --git a/common/signal_processing/documentation/img.png b/common/signal_processing/documentation/img.png new file mode 100644 index 0000000000000..8de9d193d9504 Binary files /dev/null and b/common/signal_processing/documentation/img.png differ diff --git a/common/signal_processing/include/signal_processing/butterworth.hpp b/common/signal_processing/include/signal_processing/butterworth.hpp new file mode 100644 index 0000000000000..579a915cc9968 --- /dev/null +++ b/common/signal_processing/include/signal_processing/butterworth.hpp @@ -0,0 +1,137 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIGNAL_PROCESSING__BUTTERWORTH_HPP_ +#define SIGNAL_PROCESSING__BUTTERWORTH_HPP_ + +#include +#include +#include +#include +#include + +template +const T & append_separator(const T & arg) +{ + std::cout << " "; + return arg; +} + +template +void print(Args &&... args) +{ + (std::cout << ... << append_separator(args)) << "\n"; +} + +struct sOrderCutOff +{ + int N{1}; + double Wc_rad_sec{}; // sampling frequency rad/sec + double fs{1.}; // cut-off frequency Hz +}; + +struct sDifferenceAnBn +{ + std::vector An; + std::vector Bn; +}; + +class ButterworthFilter +{ +public: + // Prints the filter order and cutoff frequency + void printFilterSpecs() const; + void printFilterContinuousTimeRoots() const; + + void printContinuousTimeTF() const; + void printDiscreteTimeTF() const; + + void Buttord(double const & Wp, double const & Ws, double const & Ap, double const & As); + + // Setters and Getters + void setCutOffFrequency(double const & Wc); // Wc is the cut-off frequency in [rad/sec] + + // fc is cut-off frequency in [Hz] and fs is the sampling frequency in [Hz] + void setCutOffFrequency(double const & fc, double const & fs); + void setOrder(int const & N); + + // Get the order, cut-off frequency and other filter properties + [[nodiscard]] sOrderCutOff getOrderCutOff() const; + [[nodiscard]] sDifferenceAnBn getAnBn() const; + + [[nodiscard]] std::vector getAn() const; + [[nodiscard]] std::vector getBn() const; + + // computes continuous time transfer function + void computeContinuousTimeTF(bool const & use_sampling_frequency = false); + + // computes continuous time transfer function + void computeDiscreteTimeTF(bool const & use_sampling_frequency = false); + +private: + // member variables + sOrderCutOff filter_specs_{}; + const double Td_{2.}; // constant of bi-linear transformation + + // Continuous time transfer function roots + struct + { + std::vector phase_angles_{}; + std::vector> continuous_time_roots_{}; + + // Continuous time transfer function numerator denominators + std::vector> continuous_time_denominator_{{0.0, 0.0}}; + double continuous_time_numerator_{0.0}; + } ct_tf_{}; + + struct + { + // Gain of the discrete time function + std::complex discrete_time_gain_{1.0, 0.0}; + + // Discrete time zeros and roots + std::vector> discrete_time_roots_{{0.0, 0.0}}; + std::vector> discrete_time_zeros_{{-1.0, 0.0}}; + + // Discrete time transfer function numerator denominators + std::vector> discrete_time_denominator_{{0.0, 0.0}}; + std::vector> discrete_time_numerator_{{0.0, 0.0}}; + } dt_tf_{}; + + // Numerator and Denominator Coefficients Bn and An of Discrete Time Filter + sDifferenceAnBn AnBn_{}; + + // METHODS + // polynomial function returns the coefficients given the roots of a polynomial + static std::vector> poly(std::vector> const & roots); + + /* + * Implementation starts by computing the pole locations of the filter in the + * polar coordinate system . The algorithm first locates the poles computing + * the phase angle and then poles as a complex number From the poles, the + * coefficients of denominator polynomial is calculated. + * + * Therefore, without phase, the roots cannot be calculated. The following + * three methods should be called successively. + * + * */ + + // computes the filter root locations in the polar coordinate system + void computePhaseAngles(); + + // Computes continuous time roots from the phase angles + void computeContinuousTimeRoots(bool const & use_sampling_frequency = false); +}; + +#endif // SIGNAL_PROCESSING__BUTTERWORTH_HPP_ diff --git a/common/signal_processing/package.xml b/common/signal_processing/package.xml index 8825ff0e31577..9496e87fc8240 100644 --- a/common/signal_processing/package.xml +++ b/common/signal_processing/package.xml @@ -5,14 +5,16 @@ 0.1.0 The signal processing package Takayuki Murooka + Ali Boyali Apache License 2.0 + Takayuki Murooka + Ali BOYALI autoware_cmake - ament_cmake_auto - geometry_msgs + rclcpp ament_cmake_ros ament_lint_auto diff --git a/common/signal_processing/src/butterworth.cpp b/common/signal_processing/src/butterworth.cpp new file mode 100644 index 0000000000000..3b4263461e9f6 --- /dev/null +++ b/common/signal_processing/src/butterworth.cpp @@ -0,0 +1,324 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "signal_processing/butterworth.hpp" + +#include + +#include +#include +#include + +/** + * @brief Computes the minimum of an analog Butterworth filter order and cut-off frequency give + * the pass and stop-band frequencies and ripple magnitude (tolerances). + * @param Wp [in] pass-band frequency [rad/sc], + * @param Ws [in] stop-band frequency [rad/sc], + * @param Ap [in] pass-band ripple [dB] + * @param As [in] stop-band ripple [dB] + * */ + +void ButterworthFilter::Buttord( + const double & Wp, const double & Ws, const double & Ap, const double & As) +{ + // N*ln(alpha) > ln(beta) + auto alpha = Ws / Wp; + auto beta = std::sqrt((std::pow(10, As / 10.0) - 1.0) / (std::pow(10, Ap / 10.0) - 1.0)); + auto order = static_cast(std::ceil(std::log(beta) / std::log(alpha))); + + setOrder(order); + + // right limit, left limit + /** + * The left and right limits of the magnitudes satisfy the specs at the + * frequencies Ws and Wp Scipy.buttord gives left limit as the cut-off frequency whereas Matlab + * gives right limit. We keep left_lim as a definition and commented out. + * */ + + double right_lim = Ws * (std::pow((std::pow(10.0, As / 10.0) - 1.0), -1.0 / (2. * order))); + // double left_lim = Wp * (std::pow((std::pow(10.0, Ap / 10.0) - 1.0), -1.0 / (2. * order))); + + setCutOffFrequency(right_lim); +} + +void ButterworthFilter::setOrder(const int & N) { filter_specs_.N = N; } + +void ButterworthFilter::setCutOffFrequency(const double & Wc) { filter_specs_.Wc_rad_sec = Wc; } + +/** + * @brief Sets the cut-off and sampling frequencies. + * @param fc [in] cut-off frequency in Hz. + * @param fs [in] sampling frequency in Hz. + * */ +void ButterworthFilter::setCutOffFrequency(const double & fc, const double & fs) +{ + /* + * fc is the cut-off frequency in [Hz] + * fs is the sampling frequency in [Hz] + * */ + if (fc >= fs / 2) { + print("Invalid argument : Cut-off frequency fc must be less than fs/2 \n"); + return; + } + + filter_specs_.Wc_rad_sec = fc * 2.0 * M_PI; + filter_specs_.fs = fs; +} + +sOrderCutOff ButterworthFilter::getOrderCutOff() const { return filter_specs_; } + +/** + * @brief Matlab equivalent : [b, a] = butter(n, Wn, 's') + * */ +void ButterworthFilter::computeContinuousTimeTF(const bool & use_sampling_frequency) +{ + // First compute the phase angles of the roots + computePhaseAngles(); + computeContinuousTimeRoots(use_sampling_frequency); + + auto cutoff_frequency_rad_sec = filter_specs_.Wc_rad_sec; + auto order = filter_specs_.N; + + ct_tf_.continuous_time_denominator_ = poly(ct_tf_.continuous_time_roots_); + ct_tf_.continuous_time_numerator_ = std::pow(cutoff_frequency_rad_sec, order); +} + +void ButterworthFilter::computePhaseAngles() +{ + const auto & order = filter_specs_.N; + ct_tf_.phase_angles_.resize(order, 0.); + + for (size_t i = 0; i < ct_tf_.phase_angles_.size(); ++i) { + auto & x = ct_tf_.phase_angles_.at(i); + x = M_PI_2 + (M_PI * (2.0 * static_cast((i + 1)) - 1.0) / (2.0 * order)); + } +} + +void ButterworthFilter::computeContinuousTimeRoots(const bool & use_sampling_frequency) +{ + const auto & order = filter_specs_.N; + const auto & sampling_frequency_hz = filter_specs_.fs; + const auto & cutoff_frequency_rad_sec = filter_specs_.Wc_rad_sec; + + ct_tf_.continuous_time_roots_.resize(order, {0.0, 0.0}); + + if (use_sampling_frequency) { + const double & Fc = (sampling_frequency_hz / M_PI) * + tan(cutoff_frequency_rad_sec / (sampling_frequency_hz * 2.0)); + + for (size_t i = 0; i < ct_tf_.continuous_time_roots_.size(); ++i) { + auto & x = ct_tf_.continuous_time_roots_[i]; + x = { + std::cos(ct_tf_.phase_angles_[i]) * Fc * 2.0 * M_PI, + std::sin(ct_tf_.phase_angles_[i]) * Fc * 2.0 * M_PI}; + } + return; + } + + for (size_t i = 0; i < ct_tf_.continuous_time_roots_.size(); ++i) { + auto & x = ct_tf_.continuous_time_roots_[i]; + x = { + cutoff_frequency_rad_sec * cos(ct_tf_.phase_angles_[i]), + cutoff_frequency_rad_sec * sin(ct_tf_.phase_angles_[i])}; + } +} +std::vector> ButterworthFilter::poly( + std::vector> const & roots) +{ + std::vector> coefficients(roots.size() + 1, {0, 0}); + + const int n{static_cast(roots.size())}; + + coefficients[0] = {1.0, 0.0}; + + for (int i = 0; i < n; i++) { + for (int j = i; j != -1; j--) { + coefficients[j + 1] = coefficients[j + 1] - (roots[i] * coefficients[j]); + } + } + + return coefficients; +} + +/** + * @brief Prints the order and cut-off angular frequency (rad/sec) of the filter + * */ + +void ButterworthFilter::printFilterContinuousTimeRoots() const +{ + std::stringstream stream; + stream << "\n The roots of the continuous-time filter Transfer Function's Denominator are : \n"; + + for (const auto & x : ct_tf_.continuous_time_roots_) { + stream << std::fixed << std::setprecision(2) << std::real(x) << " j"; + + auto txt = std::imag(x) < 0 ? " - j " : " + j "; + stream << std::fixed << std::setprecision(2) << txt << std::abs(std::imag(x)) << " \n"; + } + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "%s", stream.str().c_str()); +} +void ButterworthFilter::printContinuousTimeTF() const +{ + const auto & n = filter_specs_.N; + + RCLCPP_INFO( + rclcpp::get_logger("rclcpp"), "\nThe Continuous Time Transfer Function of the Filter is ;\n"); + + std::stringstream stream; + stream << std::fixed << std::setprecision(2) << ct_tf_.continuous_time_numerator_ << " / \n"; + + for (int i = n; i > 0; i--) { + stream << std::fixed << std::setprecision(2) + << ct_tf_.continuous_time_denominator_[n - i].real() << " * s [" << i << "] + "; + } + + stream << std::fixed << std::setprecision(2) << ct_tf_.continuous_time_denominator_[n].real(); + + const auto & tf_text = stream.str(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[%s]", tf_text.c_str()); +} + +/** + * @brief This method assumes the continuous time transfer function of filter has already been + * computed and stored in the object and uses the bilinear transformation to obtain the discrete + * time transfer function. + * + * Matlab equivalent : + * Td = 2. + * [numd, dend] = bilinear(sys_filt.Numerator{1}, sys_filt.Denominator{1}, 1/Td) + * where sys_filt is the continuous time transfer function. + * */ +void ButterworthFilter::computeDiscreteTimeTF(const bool & use_sampling_frequency) +{ + const auto & order = filter_specs_.N; + const auto & sampling_frequency_hz = filter_specs_.fs; + + // Butter puts zeros at -1.0 for causality + dt_tf_.discrete_time_zeros_.resize(order, {-1.0, 0.0}); + dt_tf_.discrete_time_roots_.resize(order, {0.0, 0.0}); + + auto & An_ = AnBn_.An; + auto & Bn_ = AnBn_.Bn; + + An_.resize(order + 1, 0.0); + Bn_.resize(order + 1, 0.0); + + dt_tf_.discrete_time_gain_ = {ct_tf_.continuous_time_numerator_, 0.0}; + + // Bi-linear Transformation of the Roots + + if (use_sampling_frequency) { + for (size_t i = 0; i < dt_tf_.discrete_time_roots_.size(); ++i) { + auto & dr = dt_tf_.discrete_time_roots_[i]; + + dr = (1.0 + ct_tf_.continuous_time_roots_[i] / (sampling_frequency_hz * 2.0)) / + (1.0 - ct_tf_.continuous_time_roots_[i] / (sampling_frequency_hz * 2.0)); + } + + dt_tf_.discrete_time_denominator_ = poly(dt_tf_.discrete_time_roots_); + + // Obtain the coefficients of numerator and denominator + dt_tf_.discrete_time_numerator_ = poly(dt_tf_.discrete_time_zeros_); + + // Compute Discrete Time Gain + const auto & sum_num = std::accumulate( + dt_tf_.discrete_time_numerator_.cbegin(), dt_tf_.discrete_time_numerator_.cend(), + std::complex{}); + + const auto & sum_den = std::accumulate( + dt_tf_.discrete_time_denominator_.cbegin(), dt_tf_.discrete_time_denominator_.cend(), + std::complex{}); + + dt_tf_.discrete_time_gain_ = std::abs(sum_den / sum_num); + + for (size_t i = 0; i < dt_tf_.discrete_time_numerator_.size(); ++i) { + auto & dn = dt_tf_.discrete_time_numerator_[i]; + dn = dn * dt_tf_.discrete_time_gain_; + Bn_[i] = dn.real(); + } + + for (size_t i = 0; i < dt_tf_.discrete_time_denominator_.size(); ++i) { + const auto & dd = dt_tf_.discrete_time_denominator_[i]; + An_[i] = dd.real(); + } + + return; + } + + for (size_t i = 0; i < dt_tf_.discrete_time_roots_.size(); ++i) { + auto & dr = dt_tf_.discrete_time_roots_[i]; + dr = (1.0 + Td_ * ct_tf_.continuous_time_roots_[i] / 2.0) / + (1.0 - Td_ * ct_tf_.continuous_time_roots_[i] / 2.0); + + dt_tf_.discrete_time_gain_ = + dt_tf_.discrete_time_gain_ / (1.0 - ct_tf_.continuous_time_roots_[i]); + } + + // Obtain the coefficients of numerator and denominator + dt_tf_.discrete_time_denominator_ = poly(dt_tf_.discrete_time_roots_); + dt_tf_.discrete_time_numerator_ = poly(dt_tf_.discrete_time_zeros_); + + for (size_t i = 0; i < dt_tf_.discrete_time_numerator_.size(); ++i) { + auto & dn = dt_tf_.discrete_time_numerator_[i]; + dn = dn * dt_tf_.discrete_time_gain_; + Bn_[i] = dn.real(); + } + + for (size_t i = 0; i < dt_tf_.discrete_time_denominator_.size(); ++i) { + const auto & dd = dt_tf_.discrete_time_denominator_[i]; + An_[i] = dd.real(); + } +} +void ButterworthFilter::printDiscreteTimeTF() const +{ + const int & n = filter_specs_.N; + + std::stringstream stream; + stream << "\nThe Discrete Time Transfer Function of the Filter is ;\n"; + + for (int i = n; i > 0; i--) { + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_numerator_[n - i].real(); + stream << " z[-" << i << " ] + "; + } + + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_numerator_[n].real() + << " / \n"; + + for (int i = n; i > 0; i--) { + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_denominator_[n - i].real(); + stream << " z[-" << i << " ] + "; + } + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_denominator_[n].real() + << " \n\n"; + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[%s]", stream.str().c_str()); +} +std::vector ButterworthFilter::getAn() const { return AnBn_.An; } +std::vector ButterworthFilter::getBn() const { return AnBn_.Bn; } +sDifferenceAnBn ButterworthFilter::getAnBn() const { return AnBn_; } + +void ButterworthFilter::printFilterSpecs() const +{ + /** + * @brief Prints the order and cut-off angular frequency (rad/sec) of the filter + * + * */ + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "The order of the filter : %d ", this->filter_specs_.N); + + RCLCPP_INFO( + rclcpp::get_logger("rclcpp"), "Cut-off Frequency : %2.2f rad/sec", + this->filter_specs_.Wc_rad_sec); +} diff --git a/common/signal_processing/test/include/butterworth_filter_test.hpp b/common/signal_processing/test/include/butterworth_filter_test.hpp new file mode 100644 index 0000000000000..6a43b0b95a7e2 --- /dev/null +++ b/common/signal_processing/test/include/butterworth_filter_test.hpp @@ -0,0 +1,30 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BUTTERWORTH_FILTER_TEST_HPP_ +#define BUTTERWORTH_FILTER_TEST_HPP_ + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "signal_processing/butterworth.hpp" + +class ButterWorthTestFixture : public ::testing::Test +{ +protected: + ButterWorthTestFixture() = default; + + ~ButterWorthTestFixture() override = default; +}; + +#endif // BUTTERWORTH_FILTER_TEST_HPP_ diff --git a/common/signal_processing/test/src/butterworth_filter_test.cpp b/common/signal_processing/test/src/butterworth_filter_test.cpp new file mode 100644 index 0000000000000..d85b9a37f870f --- /dev/null +++ b/common/signal_processing/test/src/butterworth_filter_test.cpp @@ -0,0 +1,165 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "butterworth_filter_test.hpp" + +TEST_F(ButterWorthTestFixture, butterworthOrderTest) +{ + double tol = 1e-4; + + // 1st Method + const double & Wp{2.}; // pass-band frequency [rad/sec] + const double & Ws{3.}; // stop-band frequency [rad/sec] + const double & Ap{6.}; // pass-band ripple mag or loss [dB] + const double & As{20.}; // stop band ripple attenuation [dB] + + ButterworthFilter bf; + bf.Buttord(Wp, Ws, Ap, As); + + const auto & NWc = bf.getOrderCutOff(); + // print("The computed order and frequency for the give specification : "); + // print("Minimum order N = ", NWc.N, ", and The cut-off frequency Wc = ", NWc.Wc, "rad/sec \n"); + bf.printFilterSpecs(); + + /** + * Approximate the continuous and discrete time transfer functions. + * */ + bf.computeContinuousTimeTF(); + + // Print continuous time roots. + bf.printFilterContinuousTimeRoots(); + bf.printContinuousTimeTF(); + + // Compute the discrete time transfer function. + bf.computeDiscreteTimeTF(); + bf.printDiscreteTimeTF(); + + ASSERT_EQ(5, NWc.N); + ASSERT_NEAR(1.89478, NWc.Wc_rad_sec, tol); + + // test transfer functions + bf.computeContinuousTimeTF(); + bf.computeDiscreteTimeTF(); + + const std::vector & An = bf.getAn(); + const std::vector & Bn = bf.getBn(); + + /** + * Bd = [0.1913 0.9564 1.9128 1.9128 0.9564 0.1913] + * Ad = [1.0000 1.8849 1.8881 1.0137 0.2976 0.0365] + */ + + ASSERT_NEAR(1.8849, An[1], tol); + ASSERT_NEAR(1.8881, An[2], tol); + ASSERT_NEAR(1.0137, An[3], tol); + ASSERT_NEAR(0.29762, An[4], tol); + ASSERT_NEAR(0.0365, An[5], tol); + + ASSERT_NEAR(0.9564, Bn[1], tol); + ASSERT_NEAR(1.9128, Bn[2], tol); + ASSERT_NEAR(1.9128, Bn[3], tol); + ASSERT_NEAR(0.9564, Bn[4], tol); + ASSERT_NEAR(0.1913, Bn[5], tol); +} + +TEST_F(ButterWorthTestFixture, butterDefinedSamplingOrder1) +{ + ButterworthFilter bf; + const double tol{1e-12}; + + // Test with a defined sampling frequency + const int order{1}; + const double cut_off_frq_hz{5.}; + const double sampling_frq_hz{40.}; + const bool use_sampling_frequency = true; + + // Prepare the filter + bf.setOrder(order); + bf.setCutOffFrequency(cut_off_frq_hz, sampling_frq_hz); + bf.computeContinuousTimeTF(use_sampling_frequency); + bf.computeDiscreteTimeTF(use_sampling_frequency); + + const auto & An = bf.getAn(); + const auto & Bn = bf.getBn(); + + std::vector const & An_ground_truth{1., -0.414213562373095}; + std::vector const & Bn_ground_truth{0.292893218813452, 0.292893218813452}; + + for (size_t k = 0; k < An.size(); ++k) { + ASSERT_NEAR(An[k], An_ground_truth[k], tol); + ASSERT_NEAR(Bn[k], Bn_ground_truth[k], tol); + } +} + +TEST_F(ButterWorthTestFixture, butterDefinedSamplingOrder2) +{ + ButterworthFilter bf; + const double tol{1e-12}; + + // Test with defined sampling frequency + const int order{2}; + const double cut_off_frq_hz{10.}; + const double sampling_frq_hz{100}; + const bool use_sampling_frequency = true; + + // Prepare the filter + bf.setOrder(order); + bf.setCutOffFrequency(cut_off_frq_hz, sampling_frq_hz); + bf.computeContinuousTimeTF(use_sampling_frequency); + bf.computeDiscreteTimeTF(use_sampling_frequency); + + const auto & An = bf.getAn(); + const auto & Bn = bf.getBn(); + + const std::vector & An_ground_truth{1., -1.142980502539901, 0.412801598096189}; + const std::vector & Bn_ground_truth{ + 0.067455273889072, 0.134910547778144, 0.067455273889072}; + + for (size_t k = 0; k < An.size(); ++k) { + ASSERT_NEAR(An[k], An_ground_truth[k], tol); + ASSERT_NEAR(Bn[k], Bn_ground_truth[k], tol); + } +} + +TEST_F(ButterWorthTestFixture, butterDefinedSamplingOrder3) +{ + ButterworthFilter bf; + const double tol{1e-12}; + + // Test with a defined sampling frequency + const int order{3}; + const double cut_off_frq_hz{10.}; + const double sampling_frq_hz{100}; + const bool use_sampling_frequency = true; + + // Prepare the filter + bf.setOrder(order); + bf.setCutOffFrequency(cut_off_frq_hz, sampling_frq_hz); + bf.computeContinuousTimeTF(use_sampling_frequency); + bf.computeDiscreteTimeTF(use_sampling_frequency); + + const auto & An = bf.getAn(); + const auto & Bn = bf.getBn(); + + const std::vector & An_ground_truth{ + 1., -1.760041880343169, 1.182893262037831, -0.278059917634546}; + + const std::vector & Bn_ground_truth{ + 0.018098933007514, 0.054296799022543, 0.054296799022543, 0.018098933007514}; + + for (size_t k = 0; k < An.size(); ++k) { + ASSERT_NEAR(An[k], An_ground_truth[k], tol); + ASSERT_NEAR(Bn[k], Bn_ground_truth[k], tol); + } +} diff --git a/common/signal_processing/usage_examples/main_butterworth.cpp b/common/signal_processing/usage_examples/main_butterworth.cpp new file mode 100644 index 0000000000000..2b36bd40b3578 --- /dev/null +++ b/common/signal_processing/usage_examples/main_butterworth.cpp @@ -0,0 +1,129 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "signal_processing/butterworth.hpp" + +#include +#include + +/** The filter tool can be used in the following ways. + * + * 1- Defining specs Wp, Ws, Ap, As and obtaining order and cut-off frequency (rad/sec) + * ButterworthFilter bf; + * bf.Buttord(Wp, Ws, Ap, As); Wp and Ws in [rad/sec] + * bf.computeContinuousTimeTF(); + * bf.computeDiscreteTimeTF(); + * + * 2- Defining the order and cut-off frequency (rad/sec) directly and computing the filter TFs + * bf.setOrder(N); N is integer + * bf.setCutOffFrequency(Wc); Wc in [rad/sec] + * bf.computeContinuousTimeTF(); + * bf.computeDiscreteTimeTF(); + * + * 3- Defining the order N, cut-off and sampling frequencies (Hz) + * bf.setOrder(N); N is integer + * bf.setCutOffFrequency_Hz(fc, fs); cut-off fc and sampling fs are in [Hz] + * bf.computeContinuousTimeTF(); + * bf.computeDiscreteTimeTF(); + * */ + +int main() +{ + // 1st Method + double Wp{2.}; // pass-band frequency [rad/sec] + double Ws{3.}; // stop-band frequency [rad/sec] + double Ap{6.}; // pass-band ripple mag or loss [dB] + double As{20.}; // stop band ripple attenuation [dB] + + ButterworthFilter bf1; + bf1.Buttord(Wp, Ws, Ap, As); + + auto NWc = bf1.getOrderCutOff(); + print("The computed order and frequency for the give specification : "); + print( + "Minimum order N = ", NWc.N, ", and The cut-off frequency Wc = ", NWc.Wc_rad_sec, "rad/sec \n"); + bf1.printFilterSpecs(); + + /** + * Approximate the continuous and discrete time transfer functions. + * */ + bf1.computeContinuousTimeTF(); + + // Print continuous time roots. + bf1.printFilterContinuousTimeRoots(); + bf1.printContinuousTimeTF(); + + // Compute the discrete time transfer function. + bf1.computeDiscreteTimeTF(); + bf1.printDiscreteTimeTF(); + + // 2nd METHOD + // Setting filter order N and cut-off frequency explicitly + print("SECOND TYPE of FILTER INITIALIZATION "); + + ButterworthFilter bf2; + bf2.setOrder(2); + bf2.setCutOffFrequency(2.0); + bf2.printFilterSpecs(); + + // Get the computed order and Cut-off frequency + NWc = bf2.getOrderCutOff(); + + // Print continuous time roots. + bf2.computeContinuousTimeTF(); + bf2.printFilterContinuousTimeRoots(); + bf2.printContinuousTimeTF(); + + // Compute the discrete time transfer function. + bf2.computeDiscreteTimeTF(); + bf2.printDiscreteTimeTF(); + + // 3rd METHOD + // defining a sampling frequency together with the cut-off fc, fs + print("THIRD TYPE of FILTER INITIALIZATION "); + + ButterworthFilter bf3; + bf3.setOrder(3); + + bf3.setCutOffFrequency(10, 100); + bf3.printFilterSpecs(); + + bool use_sampling_frequency{true}; + + bf3.computeContinuousTimeTF(use_sampling_frequency); + bf3.printFilterContinuousTimeRoots(); + bf3.printContinuousTimeTF(); + + // Compute Discrete Time TF + bf3.computeDiscreteTimeTF(use_sampling_frequency); + bf3.printDiscreteTimeTF(); + + auto AnBn = bf3.getAnBn(); + auto An = bf3.getAn(); + auto Bn = bf3.getBn(); + + print("An : "); + + for (double it : An) { + std::cout << std::setprecision(4) << it << ", "; + } + + print("\nBn : \n"); + + for (double it : Bn) { + std::cout << std::setprecision(4) << it << ", "; + } + + return 0; +} diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index a565fe764d1ce..6cff428fa6097 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -35,8 +35,8 @@ using LineString2d = boost::geometry::model::linestring; using LinearRing2d = boost::geometry::model::ring; using Polygon2d = boost::geometry::model::polygon; using MultiPoint2d = boost::geometry::model::multi_point; -using MultiLineString2d = boost::geometry::model::multi_linestring; -using MultiPolygon2d = boost::geometry::model::multi_polygon; +using MultiLineString2d = boost::geometry::model::multi_linestring; +using MultiPolygon2d = boost::geometry::model::multi_polygon; // 3D struct Point3d; @@ -46,8 +46,8 @@ using LineString3d = boost::geometry::model::linestring; using LinearRing3d = boost::geometry::model::ring; using Polygon3d = boost::geometry::model::polygon; using MultiPoint3d = boost::geometry::model::multi_point; -using MultiLineString3d = boost::geometry::model::multi_linestring; -using MultiPolygon3d = boost::geometry::model::multi_polygon; +using MultiLineString3d = boost::geometry::model::multi_linestring; +using MultiPolygon3d = boost::geometry::model::multi_polygon; struct Point2d : public Eigen::Vector2d { 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 b120ec4377413..3251c29718fec 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 @@ -23,6 +23,7 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/math/normalization.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include #include @@ -34,6 +35,9 @@ #include #include #include +#include +#include +#include #include @@ -731,6 +735,38 @@ geometry_msgs::msg::Pose calcInterpolatedPose( return output_pose; } + +inline geometry_msgs::msg::Vector3 createVector3(const double x, double y, double z) +{ + return geometry_msgs::build().x(x).y(y).z(z); +} + +inline geometry_msgs::msg::Twist createTwist( + const geometry_msgs::msg::Vector3 & velocity, geometry_msgs::msg::Vector3 & angular) +{ + return geometry_msgs::build().linear(velocity).angular(angular); +} + +inline double calcNorm(const geometry_msgs::msg::Vector3 & v) { return std::hypot(v.x, v.y, v.z); } + +/** + * @brief Judge whether twist covariance is valid. + * + * @param twist_with_covariance source twist with covariance + * @return If all element of covariance is 0, return false. + */ +// +inline bool isTwistCovarianceValid( + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) +{ + for (const auto & c : twist_with_covariance.covariance) { + if (c != 0.0) { + return true; + } + } + return false; +} + } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index 96f1be9de6f3f..26345236ad8ca 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -1637,3 +1637,55 @@ TEST(geometry, calcInterpolatedPose_with_Spherical_Interpolation) } } } + +TEST(geometry, getTwist) +{ + using tier4_autoware_utils::createVector3; + + geometry_msgs::msg::Vector3 velocity = createVector3(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 angular = createVector3(0.1, 0.2, 0.3); + + geometry_msgs::msg::Twist twist = geometry_msgs::build() + .linear(createVector3(1.0, 2.0, 3.0)) + .angular(createVector3(0.1, 0.2, 0.3)); + + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + twist_with_covariance.twist = geometry_msgs::build() + .linear(createVector3(1.0, 2.0, 3.0)) + .angular(createVector3(0.1, 0.2, 0.3)); + + // getTwist + { + auto t_out = tier4_autoware_utils::createTwist(velocity, angular); + EXPECT_DOUBLE_EQ(t_out.linear.x, twist.linear.x); + EXPECT_DOUBLE_EQ(t_out.linear.y, twist.linear.y); + EXPECT_DOUBLE_EQ(t_out.linear.z, twist.linear.z); + EXPECT_DOUBLE_EQ(t_out.angular.x, twist.angular.x); + EXPECT_DOUBLE_EQ(t_out.angular.y, twist.angular.y); + EXPECT_DOUBLE_EQ(t_out.angular.z, twist.angular.z); + } +} + +TEST(geometry, getTwistNorm) +{ + using tier4_autoware_utils::createVector3; + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + twist_with_covariance.twist = geometry_msgs::build() + .linear(createVector3(3.0, 4.0, 0.0)) + .angular(createVector3(0.1, 0.2, 0.3)); + EXPECT_NEAR(tier4_autoware_utils::calcNorm(twist_with_covariance.twist.linear), 5.0, epsilon); +} + +TEST(geometry, isTwistCovarianceValid) +{ + using tier4_autoware_utils::createVector3; + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + twist_with_covariance.twist = geometry_msgs::build() + .linear(createVector3(1.0, 2.0, 3.0)) + .angular(createVector3(0.1, 0.2, 0.3)); + + EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), false); + + twist_with_covariance.covariance.at(0) = 1.0; + EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), true); +} diff --git a/common/vehicle_constants_manager/CMakeLists.txt b/common/vehicle_constants_manager/CMakeLists.txt deleted file mode 100644 index 1e84c5f781d39..0000000000000 --- a/common/vehicle_constants_manager/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(vehicle_constants_manager) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(VEHICLE_CONSTANTS_MANAGER_LIB_SRC - src/vehicle_constants_manager.cpp) - -set(VEHICLE_CONSTANTS_MANAGER_LIB_HEADERS - include/vehicle_constants_manager/vehicle_constants_manager.hpp - include/vehicle_constants_manager/visibility_control.hpp) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${VEHICLE_CONSTANTS_MANAGER_LIB_SRC} - ${VEHICLE_CONSTANTS_MANAGER_LIB_HEADERS}) - -# if(BUILD_TESTING) -# set(TEST_SOURCES test/test_vehicle_constants_manager.cpp) -# set(TEST_VEHICLE_CONSTANTS_MANAGER_EXE test_vehicle_constants_manager) -# ament_add_ros_isolated_gtest(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE} ${TEST_SOURCES}) -# target_link_libraries(${TEST_VEHICLE_CONSTANTS_MANAGER_EXE} ${PROJECT_NAME}) -# endif() - -ament_auto_package(INSTALL_TO_SHARE) diff --git a/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md b/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md deleted file mode 100644 index 1a83c48a34aed..0000000000000 --- a/common/vehicle_constants_manager/design/vehicle_constants_manager-design.md +++ /dev/null @@ -1,149 +0,0 @@ -# vehicle_constants_manager - -This is the design document for the `vehicle_constants_manager` package. - -## Purpose / Use cases - - - - -This library provides a struct for holding vehicle specific constants. It also -provides a helper method to declare vehicle specific constants which have -already been passed into a node and provide a `VehicleConstants` object. - -## Design - - - - -Provided `VehicleConstants` struct holds vehicle parameters. Its parameters can -be split in 2 categories: -(The detailed descriptions and units of the variables is in the -`vehicle_constants_manager.hpp` file.) - -- Primary Constants - - wheel_radius - - wheel_width - - wheel_base - - wheel_tread - - overhang_front - - overhang_rear - - overhang_left - - overhang_right - - vehicle_height - - cg_to_rear - - tire_cornering_stiffness_front_n_per_deg - - tire_cornering_stiffness_rear_n_per_deg - - mass_vehicle - - inertia_yaw_kg_m_2 -- Derived Constants - - cg_to_front - - vehicle_length - - vehicle_width - - offset_longitudinal_min - - offset_longitudinal_max - - offset_lateral_min - - offset_lateral_max - - offset_height_min - - offset_height_max - -The `VehicleConstants` constructor is initialized with the primary parameters. - -The library also provides a `declare_and_get_vehicle_constants` method. Using -this method, the user can declare vehicle parameters that are already passed -into the node and obtain a `VehicleConstants` object. - -## Assumptions / Known limits - - - -This library assumes the vehicle is defined with Ackermann steering geometry. - -`declare_and_get_vehicle_constants` method requires the passed node to have following parameters overridden: - -(Pay attention to the `vehicle` namespace) - -```yaml -vehicle: - wheel_radius: - wheel_width: - wheel_base: - wheel_tread: - overhang_front: - overhang_rear: - overhang_left: - overhang_right: - vehicle_height: - cg_to_rear: - tire_cornering_stiffness_front_n_per_deg: - tire_cornering_stiffness_rear_n_per_deg: - mass_vehicle: - inertia_yaw_kg_m_2: -``` - -## Inputs / Outputs / API - - - - -The constructor of `VehicleConstants` takes the primary vehicle constants and -generates the derived parameters. - -`declare_and_get_vehicle_constants` method takes a `rclcpp::Node` object. And -returns a `VehicleConstants` object if it succeeds. - -Example usage: - -```cpp -// In the constructor of a node which received primary vehicle parameters from a -// .yaml file or run args. -auto vehicle_constants = declare_and_get_vehicle_constants(*this); -``` - -## Inner-workings / Algorithms - - - -Not Available. - -## Error detection and handling - - - -The `VehicleConstants` struct performs some sanity checks upon construction. - -It will throw `std::runtime_error` in case certain parameters are negative or -cg_to_rear is larger than wheel_base (to ensure center of gravity is within -front and rear axles.) - -## Security considerations - - - - -To Be Determined. - -## References / External links - - - -Not Available. - -## Future extensions / Unimplemented parts - - - -Not Available. - -## Related issues - - diff --git a/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp b/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp deleted file mode 100644 index 68ea68ca41d32..0000000000000 --- a/common/vehicle_constants_manager/include/vehicle_constants_manager/vehicle_constants_manager.hpp +++ /dev/null @@ -1,183 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// @copyright Copyright 2021 The Autoware Foundation -/// @file vehicle_constants_manager.hpp -/// @brief This file defines the vehicle_constants_manager class. - -#ifndef VEHICLE_CONSTANTS_MANAGER__VEHICLE_CONSTANTS_MANAGER_HPP_ -#define VEHICLE_CONSTANTS_MANAGER__VEHICLE_CONSTANTS_MANAGER_HPP_ - -#include -#include -#include - -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace vehicle_constants_manager -{ -/// @brief A struct that holds vehicle specific parameters that don't change over time. -/// @details These parameters include wheel size, vehicle mass, vehicle size, tire cornering -/// stiffness, moment of inertia, center of gravity. -struct VEHICLE_CONSTANTS_MANAGER_PUBLIC VehicleConstants -{ - using SharedPtr = std::shared_ptr; - using ConstSharedPtr = const SharedPtr; - - using float64_t = autoware::common::types::float64_t; - - /// @brief Construct a new instance of the measurement from a state vector. - /// @param[in] wheel_radius Radius of a wheel - /// @param[in] wheel_width Width of a wheel - /// @param[in] wheel_base Distance between front and rear axles - /// @param[in] wheel_tread Distance between centres of left and right wheels - /// @param[in] overhang_front Distance from front axle to fore-most point of the vehicle - /// @param[in] overhang_rear Distance from rear axle to rear-most point of the vehicle - /// @param[in] overhang_left Distance from left wheel center to left-most point of the vehicle - /// @param[in] overhang_right Distance from left wheel center to right-most point of the vehicle - /// @param[in] vehicle_height Distance from ground plane to the top-most point of the vehicle - /// @param[in] cg_to_rear Distance between center of gravity to rear axle - /// @param[in] tire_cornering_stiffness_front Cornering stiffness for front wheels - /// @param[in] tire_cornering_stiffness_rear Cornering stiffness for rear wheels - /// @param[in] mass_vehicle Mass of the vehicle - /// @param[in] inertia_yaw_kg_m_2 Moment of Inertia of the vehicle on Z axis - /// @param[in] maximum_turning_angle_rad Maximum turning angle for cars front axis - /// @throws std::runtime_error if certain parameters are negative. - /// @throws std::runtime_error if cg_to_rear is larger than wheel_base (center of gravity must be - /// within front and rear axles.) - explicit VehicleConstants( - float64_t wheel_radius, float64_t wheel_width, float64_t wheel_base, float64_t wheel_tread, - float64_t overhang_front, float64_t overhang_rear, float64_t overhang_left, - float64_t overhang_right, float64_t vehicle_height, float64_t cg_to_rear, - float64_t tire_cornering_stiffness_front, float64_t tire_cornering_stiffness_rear, - float64_t mass_vehicle, float64_t inertia_yaw_kg_m_2, float64_t maximum_turning_angle_rad); - - // Primary Constants - - /// @brief [m] Radius of the wheel including the tires - const float64_t wheel_radius; - - /// @brief [m] Horizontal distance between 2 circular sides of the wheel - const float64_t wheel_width; - - /// @brief [m] Absolute distance between axis centers of front and rear wheels. - const float64_t wheel_base; - - /// @brief [m] Absolute distance between axis centers of left and right wheels. - const float64_t wheel_tread; - - /// @brief [m] Absolute distance between the vertical plane passing through the centres of the - /// front wheels and the foremost point of the vehicle - const float64_t overhang_front; - - /// @brief [m] Absolute distance between the vertical plane passing through the centres of the - /// rear wheels and the rearmost point of the vehicle - const float64_t overhang_rear; - - /// @brief [m] Absolute distance between axis centers of left wheels and the leftmost point of the - /// vehicle - const float64_t overhang_left; - - /// @brief [m] Absolute distance between axis centers of right wheels and the rightmost point of - /// the vehicle - const float64_t overhang_right; - - /// @brief [m] Absolute vertical distance between ground and topmost point of the vehicle - /// including mounted sensors - const float64_t vehicle_height; - - /// @brief [m] Absolute value of longitudinal distance between Center of Gravity and center of - /// rear axle - const float64_t cg_to_rear; - - /// @brief [kg/deg] The nominal cornering stiffness is equal to the side force in Newtons divided - /// by the slip angle in Degrees for small angles for front wheels - /// @details - /// https://www.mchenrysoftware.com/medit32/readme/msmac/default.htm?turl=examplestirecorneringstiffnesscalculation1.htm - const float64_t tire_cornering_stiffness_front; - - /// @brief [kg/deg] The nominal cornering stiffness for rear wheels - const float64_t tire_cornering_stiffness_rear; - - /// @brief [kg] Total mass of the vehicle including sensors in kilograms - const float64_t mass_vehicle; - - /// @brief [kg * m^2] Moment of inertia around vertical axis of the vehicle - const float64_t inertia_yaw_kg_m2; - - /// @brief [rad] Maximum turning angle for cars front axis - const float64_t maximum_turning_angle_rad; - - // Derived Constants - - /// @brief [m] Absolute value of longitudinal distance between Center of Gravity and center of - /// front axle - const float64_t cg_to_front; - - /// @brief [m] Horizontal distance between foremost and rearmost points of the vehicle - const float64_t vehicle_length; - - /// @brief [m] Horizontal distance between leftmost and rightmost points of the vehicle - const float64_t vehicle_width; - - /// @brief Offsets from base_link - /// - /// These values assume X+ is forward, Y+ is left, Z+ is up - - /// @brief [m] Signed distance from base_link to the rear-most point of the vehicle. (Negative) - const float64_t offset_longitudinal_min; - - /// @brief [m] Signed distance from base_link to the fore-most point of the vehicle. - const float64_t offset_longitudinal_max; - - /// @brief [m] Signed distance from base_link to the right-most point of the vehicle. (Negative) - const float64_t offset_lateral_min; - - /// @brief [m] Signed distance from base_link to the left-most point of the vehicle. - const float64_t offset_lateral_max; - - /// @brief [m] Signed distance from base_link to the bottom-most point of the vehicle. (Negative) - const float64_t offset_height_min; - - /// @brief [m] Signed distance from base_link to the top-most point of the vehicle. - const float64_t offset_height_max; - - /// @brief [rad] Minimum turning radius - float64_t minimum_turning_radius; - - /// @brief Retrieves a list of vehicle parameters names and values as a string. - std::string str_pretty() const; -}; - -/// @brief Declares the vehicle parameters for the node and creates a VehicleConstants object. -/// @details It creates a `rclcpp::SyncParametersClient` object to reach parameters of the -/// `vehicle_constants_manager_node` and attempts to retrieve all required parameters from the node. -/// @throws std::runtime_error if `VehicleConstants` object fails to initialize -/// @throws rclcpp::exceptions::InvalidParameterTypeException if declare_parameter gets a value with -/// wrong type -/// @throws rclcpp::exceptions::InvalidParameterValueException if initial value fails to be set. -/// @return A VehicleConstants object containing vehicle constant parameters. -VEHICLE_CONSTANTS_MANAGER_PUBLIC VehicleConstants -declare_and_get_vehicle_constants(rclcpp::Node & node); -} // namespace vehicle_constants_manager -} // namespace common -} // namespace autoware - -#endif // VEHICLE_CONSTANTS_MANAGER__VEHICLE_CONSTANTS_MANAGER_HPP_ diff --git a/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp b/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp deleted file mode 100644 index f16c180a17173..0000000000000 --- a/common/vehicle_constants_manager/include/vehicle_constants_manager/visibility_control.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef VEHICLE_CONSTANTS_MANAGER__VISIBILITY_CONTROL_HPP_ -#define VEHICLE_CONSTANTS_MANAGER__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS) -#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllexport) -#define VEHICLE_CONSTANTS_MANAGER_LOCAL -#else // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) -// || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS) -#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __declspec(dllimport) -#define VEHICLE_CONSTANTS_MANAGER_LOCAL -#endif // defined(VEHICLE_CONSTANTS_MANAGER_BUILDING_DLL) -// || defined(VEHICLE_CONSTANTS_MANAGER_EXPORTS) -#elif defined(__linux__) -#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default"))) -#define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define VEHICLE_CONSTANTS_MANAGER_PUBLIC __attribute__((visibility("default"))) -#define VEHICLE_CONSTANTS_MANAGER_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // VEHICLE_CONSTANTS_MANAGER__VISIBILITY_CONTROL_HPP_ diff --git a/common/vehicle_constants_manager/package.xml b/common/vehicle_constants_manager/package.xml deleted file mode 100644 index 8041ae90a2cb7..0000000000000 --- a/common/vehicle_constants_manager/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - vehicle_constants_manager - 1.0.0 - Library package to help nodes access vehicle specific constants. - M. Fatih Cırıt - Apache 2.0 - - ament_cmake_auto - - autoware_cmake - - autoware_auto_common - rclcpp - rclcpp_components - - ament_cmake_ros - - - ament_cmake - - diff --git a/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml b/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml deleted file mode 100644 index ea9a6f10d5abc..0000000000000 --- a/common/vehicle_constants_manager/param/params_lexus_rx_hybrid_2016.yaml +++ /dev/null @@ -1,18 +0,0 @@ -/**: - ros__parameters: - vehicle: - wheel_radius: 0.37 # Measured from SVL - wheel_width: 0.27 # Measured from SVL - wheel_base: 2.734 # Measured from SVL - wheel_tread: 1.571 # Measured from SVL - overhang_front: 1.033 # Measured from SVL - overhang_rear: 1.021 # Measured from SVL - overhang_left: 0.3135 # Measured from SVL - overhang_right: 0.3135 # Measured from SVL - vehicle_height: 1.662 # Measured from SVL - cg_to_rear: 1.367 # Assuming it is in middle of front and rear axle centers - tire_cornering_stiffness_front: 0.1 # Taken from AVP demo params, can't verify - tire_cornering_stiffness_rear: 0.1 # Taken from AVP demo params, can't verify - mass_vehicle: 2120.0 # Measured from SVL - inertia_yaw_kg_m2: 12.0 # Taken from AVP demo params, can't verify - maximum_turning_angle_rad: 0.53 diff --git a/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp b/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp deleted file mode 100644 index 97ea7ae5dd4eb..0000000000000 --- a/common/vehicle_constants_manager/src/vehicle_constants_manager.cpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "vehicle_constants_manager/vehicle_constants_manager.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace common -{ -namespace vehicle_constants_manager -{ - -using float64_t = VehicleConstants::float64_t; - -VehicleConstants::VehicleConstants( - float64_t wheel_radius, float64_t wheel_width, float64_t wheel_base, float64_t wheel_tread, - float64_t overhang_front, float64_t overhang_rear, float64_t overhang_left, - float64_t overhang_right, float64_t vehicle_height, float64_t cg_to_rear, - float64_t tire_cornering_stiffness_front, float64_t tire_cornering_stiffness_rear, - float64_t mass_vehicle, float64_t inertia_yaw_kg_m_2, float64_t maximum_turning_angle_rad) -: wheel_radius(wheel_radius), - wheel_width(wheel_width), - wheel_base(wheel_base), - wheel_tread(wheel_tread), - overhang_front(overhang_front), - overhang_rear(overhang_rear), - overhang_left(overhang_left), - overhang_right(overhang_right), - vehicle_height(vehicle_height), - cg_to_rear(cg_to_rear), - tire_cornering_stiffness_front(tire_cornering_stiffness_front), - tire_cornering_stiffness_rear(tire_cornering_stiffness_rear), - mass_vehicle(mass_vehicle), - inertia_yaw_kg_m2(inertia_yaw_kg_m_2), - maximum_turning_angle_rad(maximum_turning_angle_rad), - cg_to_front(wheel_base - cg_to_rear), - vehicle_length(overhang_front + wheel_base + overhang_rear), - vehicle_width(overhang_left + wheel_tread + overhang_right), - offset_longitudinal_min(-overhang_rear), - offset_longitudinal_max(wheel_base + overhang_front), - offset_lateral_min(-(wheel_tread / 2.0 + overhang_right)), - offset_lateral_max(wheel_tread / 2.0 + overhang_left), - offset_height_min(-wheel_radius), - offset_height_max(vehicle_height - wheel_radius) -{ - // Sanity Checks - // Center of gravity must be between front and rear axles - if (wheel_base < cg_to_rear) { - throw std::runtime_error("wheel_base must be larger than cg_to_rear"); - } - - // These values must be positive - auto throw_if_negative = [](float64_t number, const std::string & name) { - if (number < 0.0) { - throw std::runtime_error(name + " = " + std::to_string(number) + " shouldn't be negative."); - } - }; - throw_if_negative(wheel_radius, "wheel_radius"); - throw_if_negative(wheel_width, "wheel_width"); - throw_if_negative(wheel_base, "wheel_base"); - throw_if_negative(wheel_tread, "wheel_tread"); - throw_if_negative(overhang_front, "overhang_front"); - throw_if_negative(overhang_rear, "overhang_rear"); - throw_if_negative(overhang_left, "overhang_left"); - throw_if_negative(overhang_right, "overhang_right"); - throw_if_negative(vehicle_height, "vehicle_height"); - throw_if_negative(cg_to_rear, "cg_to_rear"); - throw_if_negative(tire_cornering_stiffness_front, "tire_cornering_stiffness_front"); - throw_if_negative(tire_cornering_stiffness_rear, "tire_cornering_stiffness_rear"); - throw_if_negative(mass_vehicle, "mass_vehicle"); - throw_if_negative(inertia_yaw_kg_m_2, "inertia_yaw_kg_m_2"); - throw_if_negative(maximum_turning_angle_rad, "maximum_turning_angle_rad"); - - if (!(0.0 < maximum_turning_angle_rad && maximum_turning_angle_rad < (M_PI / 2.0))) { - throw std::runtime_error( - "maximum_turning_angle_rad must be positive and cannot be greater than 0.5*PI."); - } - minimum_turning_radius = wheel_base / tan(maximum_turning_angle_rad); -} - -std::string VehicleConstants::str_pretty() const -{ - return std::string{ - "wheel_radius: " + std::to_string(wheel_radius) + - "\n" - "wheel_width: " + - std::to_string(wheel_width) + - "\n" - "wheel_base: " + - std::to_string(wheel_base) + - "\n" - "wheel_tread: " + - std::to_string(wheel_tread) + - "\n" - "overhang_front: " + - std::to_string(overhang_front) + - "\n" - "overhang_rear: " + - std::to_string(overhang_rear) + - "\n" - "overhang_left: " + - std::to_string(overhang_left) + - "\n" - "overhang_right: " + - std::to_string(overhang_right) + - "\n" - "vehicle_height: " + - std::to_string(vehicle_height) + - "\n" - "cg_to_rear: " + - std::to_string(cg_to_rear) + - "\n" - "tire_cornering_stiffness_front: " + - std::to_string(tire_cornering_stiffness_front) + - "\n" - "tire_cornering_stiffness_rear: " + - std::to_string(tire_cornering_stiffness_rear) + - "\n" - "mass_vehicle: " + - std::to_string(mass_vehicle) + - "\n" - "inertia_yaw_kg_m2: " + - std::to_string(inertia_yaw_kg_m2) + - "\n" - "maximum_turning_angle_rad: " + - std::to_string(maximum_turning_angle_rad) + - "\n" - "cg_to_front: " + - std::to_string(cg_to_front) + - "\n" - "vehicle_length: " + - std::to_string(vehicle_length) + - "\n" - "vehicle_width: " + - std::to_string(vehicle_width) + - "\n" - "offset_longitudinal_min: " + - std::to_string(offset_longitudinal_min) + - "\n" - "offset_longitudinal_max: " + - std::to_string(offset_longitudinal_max) + - "\n" - "offset_lateral_min: " + - std::to_string(offset_lateral_min) + - "\n" - "offset_lateral_max: " + - std::to_string(offset_lateral_max) + - "\n" - "offset_height_min: " + - std::to_string(offset_height_min) + - "\n" - "offset_height_max: " + - std::to_string(offset_height_max) + - "\n" - "minimum_turning_radius: " + - std::to_string(minimum_turning_radius) + "\n"}; -} - -VehicleConstants declare_and_get_vehicle_constants(rclcpp::Node & node) -{ - // Initialize the parameters - const std::string ns = "vehicle."; - std::map params{ - std::make_pair(ns + "wheel_radius", -1.0), - std::make_pair(ns + "wheel_width", -1.0), - std::make_pair(ns + "wheel_base", -1.0), - std::make_pair(ns + "wheel_tread", -1.0), - std::make_pair(ns + "overhang_front", -1.0), - std::make_pair(ns + "overhang_rear", -1.0), - std::make_pair(ns + "overhang_left", -1.0), - std::make_pair(ns + "overhang_right", -1.0), - std::make_pair(ns + "vehicle_height", -1.0), - std::make_pair(ns + "cg_to_rear", -1.0), - std::make_pair(ns + "tire_cornering_stiffness_front", -1.0), - std::make_pair(ns + "tire_cornering_stiffness_rear", -1.0), - std::make_pair(ns + "mass_vehicle", -1.0), - std::make_pair(ns + "inertia_yaw_kg_m2", -1.0), - std::make_pair(ns + "maximum_turning_angle_rad", -1.0)}; - - // Try to get parameter values from parameter_overrides set either from .yaml - // or with args. - for (auto & pair : params) { - // If it is already declared - if (node.has_parameter(pair.first)) { - node.get_parameter(pair.first, pair.second); - continue; - } - - pair.second = - node.declare_parameter(pair.first, rclcpp::ParameterType::PARAMETER_DOUBLE).get(); - } - - return VehicleConstants( - params.at(ns + "wheel_radius"), params.at(ns + "wheel_width"), params.at(ns + "wheel_base"), - params.at(ns + "wheel_tread"), params.at(ns + "overhang_front"), - params.at(ns + "overhang_rear"), params.at(ns + "overhang_left"), - params.at(ns + "overhang_right"), params.at(ns + "vehicle_height"), - params.at(ns + "cg_to_rear"), params.at(ns + "tire_cornering_stiffness_front"), - params.at(ns + "tire_cornering_stiffness_rear"), params.at(ns + "mass_vehicle"), - params.at(ns + "inertia_yaw_kg_m2"), params.at(ns + "maximum_turning_angle_rad")); -} -} // namespace vehicle_constants_manager -} // namespace common -} // namespace autoware diff --git a/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp b/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp deleted file mode 100644 index 00f34b0bfeb81..0000000000000 --- a/common/vehicle_constants_manager/test/test_vehicle_constants_manager.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "gtest/gtest.h" -#include "vehicle_constants_manager/vehicle_constants_manager.hpp" - -#include -#include -#include -#include - -TEST(TestVehicleConstantsManager, TestInitializationConstructor) -{ - using float64_t = autoware::common::types::float64_t; - using VehicleConstants = autoware::common::vehicle_constants_manager::VehicleConstants; - - const float64_t wheel_radius = 0.37; - const float64_t wheel_width = 0.27; - const float64_t wheel_base = 2.734; - const float64_t wheel_tread = 1.571; - const float64_t overhang_front = 1.033; - const float64_t overhang_rear = 1.021; - const float64_t overhang_left = 0.3135; - const float64_t overhang_right = 0.3135; - const float64_t vehicle_height = 1.662; - const float64_t cg_to_rear = 1.367; - const float64_t tire_cornering_stiffness_front_n_per_deg = 0.1; - const float64_t tire_cornering_stiffness_rear_n_per_deg = 0.1; - const float64_t mass_vehicle = 2120.0; - const float64_t inertia_yaw_kg_m_2 = 12.0; - const float64_t maximum_turning_angle_rad = 0.53; - - // Well set parameters - EXPECT_NO_THROW(VehicleConstants vc( - wheel_radius, wheel_width, wheel_base, wheel_tread, overhang_front, overhang_rear, - overhang_left, overhang_right, vehicle_height, cg_to_rear, - tire_cornering_stiffness_front_n_per_deg, tire_cornering_stiffness_rear_n_per_deg, mass_vehicle, - inertia_yaw_kg_m_2, maximum_turning_angle_rad)); - - // Center of gravity is not within wheel_base - const float64_t bad_cg_to_rear = wheel_base + 1.0; - EXPECT_THROW( - VehicleConstants vc( - wheel_radius, wheel_width, wheel_base, wheel_tread, overhang_front, overhang_rear, - overhang_left, overhang_right, vehicle_height, bad_cg_to_rear, - tire_cornering_stiffness_front_n_per_deg, tire_cornering_stiffness_rear_n_per_deg, - mass_vehicle, inertia_yaw_kg_m_2, maximum_turning_angle_rad), - std::runtime_error); - - // Some supposedly absolute parameters are negative - EXPECT_THROW( - VehicleConstants vc( - wheel_radius, wheel_width, -wheel_base, wheel_tread, -overhang_front, overhang_rear, - overhang_left, -overhang_right, vehicle_height, cg_to_rear, - tire_cornering_stiffness_front_n_per_deg, tire_cornering_stiffness_rear_n_per_deg, - mass_vehicle, inertia_yaw_kg_m_2, maximum_turning_angle_rad), - std::runtime_error); -} - -TEST(TestVehicleConstantsManager, TestGetVehicleConstantsMissing) -{ - rclcpp::init(0, nullptr); - const std::string ns_node = "TestGetVehicleConstantsMissing"; - rclcpp::Node node("some_node", ns_node); - EXPECT_THROW( - autoware::common::vehicle_constants_manager::declare_and_get_vehicle_constants(node), - std::runtime_error); - rclcpp::shutdown(); -} - -TEST(TestVehicleConstantsManager, TestGetVehicleConstants) -{ - rclcpp::init(0, nullptr); - const std::string ns_node = "TestGetVehicleConstants"; - std::vector params; - const std::string ns_vehicle = "vehicle."; - params.emplace_back(ns_vehicle + "wheel_radius", 0.37); - params.emplace_back(ns_vehicle + "wheel_width", 0.27); - params.emplace_back(ns_vehicle + "wheel_base", 2.734); - params.emplace_back(ns_vehicle + "wheel_tread", 1.571); - params.emplace_back(ns_vehicle + "overhang_front", 1.033); - params.emplace_back(ns_vehicle + "overhang_rear", 1.021); - params.emplace_back(ns_vehicle + "overhang_left", 0.3135); - params.emplace_back(ns_vehicle + "overhang_right", 0.3135); - params.emplace_back(ns_vehicle + "vehicle_height", 1.662); - params.emplace_back(ns_vehicle + "cg_to_rear", 1.367); - params.emplace_back(ns_vehicle + "tire_cornering_stiffness_front", 0.1); - params.emplace_back(ns_vehicle + "tire_cornering_stiffness_rear", 0.1); - params.emplace_back(ns_vehicle + "mass_vehicle", 2120.0); - params.emplace_back(ns_vehicle + "inertia_yaw_kg_m2", 12.0); - params.emplace_back(ns_vehicle + "maximum_turning_angle_rad", 0.53); - - rclcpp::NodeOptions node_options; - node_options.parameter_overrides(params); - - const rclcpp::Node::SharedPtr node = - std::make_shared("some_node", ns_node, node_options); - - EXPECT_NO_THROW( - autoware::common::vehicle_constants_manager::declare_and_get_vehicle_constants(*node)); - - rclcpp::shutdown(); -} diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 1bd8e24869b02..7abab16cf42f4 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -163,7 +163,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec const auto remain_distance = length - total_length; // Over length - if (remain_distance <= 0) { + if (remain_distance <= 0.0) { break; } @@ -239,7 +239,9 @@ bool ObstacleCollisionChecker::willCollide( const pcl::PointCloud & obstacle_pointcloud, const std::vector & vehicle_footprints) { - for (const auto & vehicle_footprint : vehicle_footprints) { + for (size_t i = 1; i < vehicle_footprints.size(); i++) { + // skip first footprint because surround obstacle checker handle it + const auto & vehicle_footprint = vehicle_footprints.at(i); if (hasCollision(obstacle_pointcloud, vehicle_footprint)) { RCLCPP_WARN( rclcpp::get_logger("obstacle_collision_checker"), "ObstacleCollisionChecker::willCollide"); @@ -254,7 +256,7 @@ bool ObstacleCollisionChecker::hasCollision( const pcl::PointCloud & obstacle_pointcloud, const LinearRing2d & vehicle_footprint) { - for (const auto & point : obstacle_pointcloud) { + for (const auto & point : obstacle_pointcloud.points) { if (boost::geometry::within( tier4_autoware_utils::Point2d{point.x, point.y}, vehicle_footprint)) { RCLCPP_WARN( 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 cc819dc4ba768..2322569673cf3 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 @@ -28,16 +28,21 @@ namespace { -template -void update_param( - const std::vector & parameters, const std::string & name, T & value) +template +bool update_param( + const std::vector & params, const std::string & name, T & value) { - auto it = std::find_if( - parameters.cbegin(), parameters.cend(), - [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); - if (it != parameters.cend()) { - value = it->template get_value(); + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + + // Not found + if (itr == params.cend()) { + return false; } + + value = itr->template get_value(); + return true; } } // namespace @@ -71,7 +76,7 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt transform_listener_ = std::make_shared(this); sub_obstacle_pointcloud_ = create_subscription( - "input/obstacle_pointcloud", 1, + "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); sub_reference_trajectory_ = create_subscription( "input/reference_trajectory", 1, @@ -194,8 +199,15 @@ void ObstacleCollisionCheckerNode::onTimer() current_pose_ = self_pose_listener_->getCurrentPose(); if (obstacle_pointcloud_) { const auto & header = obstacle_pointcloud_->header; - obstacle_transform_ = transform_listener_->getTransform( - "map", header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); + try { + obstacle_transform_ = transform_listener_->getTransform( + "map", header.frame_id, header.stamp, rclcpp::Duration::from_seconds(0.01)); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform map to %s: %s", header.frame_id.c_str(), + ex.what()); + return; + } } if (!isDataReady()) { @@ -229,14 +241,23 @@ rcl_interfaces::msg::SetParametersResult ObstacleCollisionCheckerNode::paramCall result.successful = true; result.reason = "success"; - Param param; try { - update_param(parameters, "delay_time", param.delay_time); - update_param(parameters, "footprint_margin", param.footprint_margin); - update_param(parameters, "max_deceleration", param.max_deceleration); - update_param(parameters, "resample_interval", param.resample_interval); - update_param(parameters, "search_radius", param.search_radius); - param_ = param; + // Node Parameter + { + auto & p = node_param_; + + // Update params + update_param(parameters, "update_rate", p.update_rate); + } + + auto & p = param_; + + update_param(parameters, "delay_time", p.delay_time); + update_param(parameters, "footprint_margin", p.footprint_margin); + update_param(parameters, "max_deceleration", p.max_deceleration); + update_param(parameters, "resample_interval", p.resample_interval); + update_param(parameters, "search_radius", p.search_radius); + if (obstacle_collision_checker_) { obstacle_collision_checker_->setParam(param_); } diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index 15cc54979f9d6..294ff8226ff84 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -38,6 +38,10 @@ #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "trajectory_follower/lateral_controller_base.hpp" +#include +#include +#include + #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -53,9 +57,18 @@ using autoware::motion::control::trajectory_follower::InputData; using autoware::motion::control::trajectory_follower::LateralControllerBase; using autoware::motion::control::trajectory_follower::LateralOutput; using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; namespace pure_pursuit { + +struct PpOutput +{ + double curvature; + double velocity; +}; + struct Param { // Global Parameters @@ -67,6 +80,9 @@ struct Param double min_lookahead_distance; double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear double converged_steer_rad_; + double prediction_ds; + double prediction_distance_length; // Total distance of prediction trajectory + double resampling_ds; }; struct DebugData @@ -82,16 +98,25 @@ class PurePursuitLateralController : public LateralControllerBase private: rclcpp::Node::SharedPtr node_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; - + boost::optional> output_tp_array_; + autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_; + boost::optional prev_cmd_; + + // Predicted Trajectory publish + rclcpp::Publisher::SharedPtr + pub_predicted_trajectory_; bool isDataReady(); void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void setResampledTrajectory(); + // TF tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -106,6 +131,7 @@ class PurePursuitLateralController : public LateralControllerBase * @brief compute control command for path follow with a constant control period */ boost::optional run() override; + AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature); /** @@ -114,13 +140,30 @@ class PurePursuitLateralController : public LateralControllerBase void setInputData(InputData const & input_data) override; // Parameter - Param param_; + Param param_{}; // Algorithm std::unique_ptr pure_pursuit_; - boost::optional calcTargetCurvature(); - boost::optional calcTargetPoint() const; + boost::optional calcTargetCurvature( + bool is_control_output, geometry_msgs::msg::Pose pose); + + boost::optional calcTargetPoint( + geometry_msgs::msg::Pose pose) const; + + /** + * @brief It takes current pose, control command, and delta distance. Then it calculates next pose + * of vehicle. + */ + + TrajectoryPoint calcNextPose( + const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const; + + boost::optional generatePredictedTrajectory(); + + boost::optional generateOutputControlCmd(); + + bool calcIsSteerConverged(const AckermannLateralCommand & cmd); // Debug mutable DebugData debug_data_; diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp index 4beb0e06db6ef..5441eaa200ac2 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp +++ b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp @@ -50,7 +50,9 @@ namespace pure_pursuit namespace planning_utils { constexpr double ERROR = 1e-6; - +double calcArcLengthFromWayPoint( + const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const size_t dst_idx); double calcCurvature( const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & curr_pose); double calcDistance2D(const geometry_msgs::msg::Point & p, const geometry_msgs::msg::Point & q); diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 0cd68d0749ee0..b611ccb4f4957 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -7,6 +7,7 @@ Takamasa Horibe Apache License 2.0 + Berkay Karaman Kenji Miyake Takamasa Horibe @@ -16,7 +17,12 @@ autoware_auto_control_msgs autoware_auto_planning_msgs + boost geometry_msgs + libboost-dev + motion_common + motion_utils + nav_msgs rclcpp rclcpp_components sensor_msgs diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index e836d3da84337..9328e6d24455d 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -70,10 +70,17 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) param_.reverse_min_lookahead_distance = node_->declare_parameter("reverse_min_lookahead_distance", 7.0); param_.converged_steer_rad_ = node_->declare_parameter("converged_steer_rad", 0.1); + param_.prediction_ds = node_->declare_parameter("prediction_ds", 0.3); + param_.prediction_distance_length = + node_->declare_parameter("prediction_distance_length", 21.0); + param_.resampling_ds = node_->declare_parameter("resampling_ds", 0.1); // Debug Publishers pub_debug_marker_ = node_->create_publisher("~/debug/markers", 0); + // Publish predicted trajectory + pub_predicted_trajectory_ = node_->create_publisher( + "~/output/predicted_trajectory", 1); // Wait for first current pose tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); @@ -99,6 +106,12 @@ bool PurePursuitLateralController::isDataReady() return false; } + if (!current_steering_) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_steering..."); + return false; + } + return true; } @@ -109,32 +122,171 @@ void PurePursuitLateralController::setInputData(InputData const & input_data) current_steering_ = input_data.current_steering_ptr; } +TrajectoryPoint PurePursuitLateralController::calcNextPose( + const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const +{ + geometry_msgs::msg::Transform transform; + transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = + planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); + TrajectoryPoint output_p; + + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(point.pose, tf_pose); + tf2::toMsg(tf_pose * tf_offset, output_p.pose); + return output_p; +} + +void PurePursuitLateralController::setResampledTrajectory() +{ + // Interpolate with constant interval distance. + std::vector out_arclength; + const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(*trajectory_); + const auto traj_length = motion_utils::calcArcLength(input_tp_array); + for (double s = 0; s < traj_length; s += param_.resampling_ds) { + out_arclength.push_back(s); + } + trajectory_resampled_ = + std::make_shared(motion_utils::resampleTrajectory( + motion_utils::convertToTrajectory(input_tp_array), out_arclength)); + trajectory_resampled_->points.back() = trajectory_->points.back(); + trajectory_resampled_->header = trajectory_->header; + output_tp_array_ = boost::optional>( + motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_)); +} + +boost::optional PurePursuitLateralController::generatePredictedTrajectory() +{ + const auto closest_idx_result = + motion_utils::findNearestIndex(*output_tp_array_, current_pose_->pose, 3.0, M_PI_4); + + if (!closest_idx_result) { + return boost::none; + } + + const double remaining_distance = planning_utils::calcArcLengthFromWayPoint( + *trajectory_resampled_, *closest_idx_result, trajectory_resampled_->points.size() - 1); + + const auto num_of_iteration = std::max( + static_cast(std::ceil( + std::min(remaining_distance, param_.prediction_distance_length) / param_.prediction_ds)), + 1); + Trajectory predicted_trajectory; + + // Iterative prediction: + for (int i = 0; i < num_of_iteration; i++) { + if (i == 0) { + // For first point, use the odometry for velocity, and use the current_pose for prediction. + + TrajectoryPoint p; + p.pose = current_pose_->pose; + p.longitudinal_velocity_mps = current_odometry_->twist.twist.linear.x; + predicted_trajectory.points.push_back(p); + + const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); + AckermannLateralCommand tmp_msg; + + if (pp_output) { + tmp_msg = generateCtrlCmdMsg(pp_output->curvature); + predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "failed to solve pure_pursuit for prediction"); + tmp_msg = generateCtrlCmdMsg(0.0); + } + TrajectoryPoint p2; + p2 = calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg); + predicted_trajectory.points.push_back(p2); + + } else { + const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); + AckermannLateralCommand tmp_msg; + + if (pp_output) { + tmp_msg = generateCtrlCmdMsg(pp_output->curvature); + predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "failed to solve pure_pursuit for prediction"); + tmp_msg = generateCtrlCmdMsg(0.0); + } + predicted_trajectory.points.push_back( + calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg)); + } + } + + // for last point + predicted_trajectory.points.back().longitudinal_velocity_mps = 0.0; + predicted_trajectory.header.frame_id = trajectory_resampled_->header.frame_id; + predicted_trajectory.header.stamp = trajectory_resampled_->header.stamp; + + return predicted_trajectory; +} + boost::optional PurePursuitLateralController::run() { current_pose_ = self_pose_listener_.getCurrentPose(); if (!isDataReady()) { return boost::none; } + setResampledTrajectory(); + if (!output_tp_array_ || !trajectory_resampled_) { + return boost::none; + } + const auto cmd_msg = generateOutputControlCmd(); + if (!cmd_msg) { + RCLCPP_ERROR(node_->get_logger(), "Failed to generate control command."); + return boost::none; + } - const auto target_curvature = calcTargetCurvature(); - AckermannLateralCommand cmd_msg; - if (target_curvature) { - cmd_msg = generateCtrlCmdMsg(*target_curvature); - publishDebugMarker(); + LateralOutput output; + output.control_cmd = *cmd_msg; + output.sync_data.is_steer_converged = calcIsSteerConverged(*cmd_msg); + + // calculate predicted trajectory with iterative calculation + const auto predicted_trajectory = generatePredictedTrajectory(); + if (!predicted_trajectory) { + RCLCPP_ERROR(node_->get_logger(), "Failed to generate predicted trajectory."); } else { - RCLCPP_WARN_THROTTLE( - node_->get_logger(), *node_->get_clock(), 5000, "failed to solve pure_pursuit"); - cmd_msg = generateCtrlCmdMsg({0.0}); + pub_predicted_trajectory_->publish(*predicted_trajectory); } - LateralOutput output; - output.control_cmd = cmd_msg; - output.sync_data.is_steer_converged = - std::abs(cmd_msg.steering_tire_angle - current_steering_->steering_tire_angle) < - static_cast(param_.converged_steer_rad_); return output; } +bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd) +{ + return std::abs(cmd.steering_tire_angle - current_steering_->steering_tire_angle) < + static_cast(param_.converged_steer_rad_); +} + +boost::optional PurePursuitLateralController::generateOutputControlCmd() +{ + // Generate the control command + const auto pp_output = calcTargetCurvature(true, current_pose_->pose); + AckermannLateralCommand output_cmd; + + if (pp_output) { + output_cmd = generateCtrlCmdMsg(pp_output->curvature); + prev_cmd_ = boost::optional(output_cmd); + publishDebugMarker(); + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "failed to solve pure_pursuit for control command calculation"); + if (prev_cmd_) { + output_cmd = *prev_cmd_; + } else { + output_cmd = generateCtrlCmdMsg(0.0); + } + } + return output_cmd; +} + AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( const double target_curvature) { @@ -160,17 +312,18 @@ void PurePursuitLateralController::publishDebugMarker() const pub_debug_marker_->publish(marker_array); } -boost::optional PurePursuitLateralController::calcTargetCurvature() +boost::optional PurePursuitLateralController::calcTargetCurvature( + bool is_control_output, geometry_msgs::msg::Pose pose) { // Ignore invalid trajectory - if (trajectory_->points.size() < 3) { + if (trajectory_resampled_->points.size() < 3) { RCLCPP_WARN_THROTTLE( node_->get_logger(), *node_->get_clock(), 5000, "received path size is < 3, ignored"); return {}; } // Calculate target point for velocity/acceleration - const auto target_point = calcTargetPoint(); + const auto target_point = calcTargetPoint(pose); if (!target_point) { return {}; } @@ -181,13 +334,19 @@ boost::optional PurePursuitLateralController::calcTargetCurvature() const bool is_reverse = (target_vel < 0); const double min_lookahead_distance = is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; - const double lookahead_distance = calcLookaheadDistance( - current_odometry_->twist.twist.linear.x, param_.lookahead_distance_ratio, - min_lookahead_distance); + double lookahead_distance = min_lookahead_distance; + if (is_control_output) { + lookahead_distance = calcLookaheadDistance( + current_odometry_->twist.twist.linear.x, param_.lookahead_distance_ratio, + min_lookahead_distance); + } else { + lookahead_distance = + calcLookaheadDistance(target_vel, param_.lookahead_distance_ratio, min_lookahead_distance); + } // Set PurePursuit data - pure_pursuit_->setCurrentPose(current_pose_->pose); - pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_)); + pure_pursuit_->setCurrentPose(pose); + pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_resampled_)); pure_pursuit_->setLookaheadDistance(lookahead_distance); // Run PurePursuit @@ -199,22 +358,30 @@ boost::optional PurePursuitLateralController::calcTargetCurvature() const auto kappa = pure_pursuit_result.second; // Set debug data - debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); + if (is_control_output) { + debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); + } + PpOutput output{}; + output.curvature = kappa; + if (!is_control_output) { + output.velocity = current_odometry_->twist.twist.linear.x; + } else { + output.velocity = target_vel; + } - return kappa; + return output; } boost::optional -PurePursuitLateralController::calcTargetPoint() const +PurePursuitLateralController::calcTargetPoint(geometry_msgs::msg::Pose pose) const { - const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( - planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); - - if (!closest_idx_result.first) { + const auto closest_idx_result = + motion_utils::findNearestIndex(*output_tp_array_, pose, 3.0, M_PI_4); + if (!closest_idx_result) { RCLCPP_ERROR(node_->get_logger(), "cannot find closest waypoint"); return {}; } - return trajectory_->points.at(closest_idx_result.second); + return trajectory_resampled_->points.at(*closest_idx_result); } } // namespace pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp index 4759844d95569..c873516d87694 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp @@ -22,6 +22,21 @@ namespace pure_pursuit { namespace planning_utils { +double calcArcLengthFromWayPoint( + const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const size_t dst_idx) +{ + double length = 0; + for (size_t i = src_idx; i < dst_idx; ++i) { + const double dx_wp = + input_path.points.at(i + 1).pose.position.x - input_path.points.at(i).pose.position.x; + const double dy_wp = + input_path.points.at(i + 1).pose.position.y - input_path.points.at(i).pose.position.y; + length += std::hypot(dx_wp, dy_wp); + } + return length; +} + double calcCurvature( const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index d5c1f868c005a..639157b0f9e0d 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -412,7 +412,8 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Check pause pause_->update(filtered_commands.control); if (pause_->is_paused()) { - filtered_commands.control = createStopControlCmd(); + filtered_commands.control.longitudinal.speed = 0.0; + filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_; } // Apply limit filtering diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 8d9b3d90d9fa1..900e1c55b48dc 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -12,6 +12,7 @@ autoware_cmake external_cmd_converter + external_cmd_selector lane_departure_checker shift_decider trajectory_follower diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml new file mode 100644 index 0000000000000..c6a0c275db0b4 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml @@ -0,0 +1,33 @@ +/**: + ros__parameters: + min_ttc: 1.0 # [s] minimum time without collision when doing projection at each trajectory point + distance_buffer: 0.0 # [m] extra distance to add to a projection (in addition to the vehicle overhang) + min_adjusted_velocity: 2.5 # [m/s] minimum velocity that the module can set + max_deceleration: 2.0 # [m/s²] maximum deceleration caused by the adjusted velocity + + trajectory_preprocessing: + start_distance: 0.0 # [m] distance ahead of ego from which to start modifying the trajectory + max_length: 100.0 # [m] maximum distance (from the start point) where the velocity is adjusted + max_duration: 10.0 # [s] maximum duration (from the start point) where the velocity is adjusted + downsample_factor: 10 # factor by which to downsample the input trajectory for calculation + calculate_steering_angles: false # if true, the steering angle at each trajectory point is calculated from the change in heading + + simulation: + model: particle # model to use for forward projection at each trajectory point. Either "particle" or "bicycle" + distance_method: exact # distance calculation method. Either "exact" or "approximation". + # parameters used only with the bicycle model + steering_offset: 0.01 # [rad] steering angle offset used to model uncertainty in the forward projection + nb_points: 5 # number of points representing the curved projections + + obstacles: + dynamic_source: static_only # source of dynamic obstacles. Must be 'pointcloud', 'occupancy_grid', or 'static_only'. + ignore_obstacles_on_path: false # if true, obstacles on the ego paths are ignored + ignore_extra_distance: 1.0 # [m] extra lateral distance where obstacles along the path are ignored + occupancy_grid_threshold: 60 # occupancy grid values higher than this are considered to be obstacles + dynamic_obstacles_buffer: 1.5 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection + dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module + static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles + - guard_rail + filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles + rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection + rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 99ab74f0856fe..91bc713796188 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -87,6 +87,40 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # obstacle velocity limiter + obstacle_velocity_limiter_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "lane_driving", + "motion_planning", + "obstacle_velocity_limiter", + "obstacle_velocity_limiter.param.yaml", + ) + with open(obstacle_velocity_limiter_param_path, "r") as f: + obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"] + obstacle_velocity_limiter_component = ComposableNode( + package="obstacle_velocity_limiter", + plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode", + name="obstacle_velocity_limiter", + namespace="", + remappings=[ + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/dynamic_obstacles", "/perception/object_recognition/objects"), + ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"), + ("~/input/obstacle_pointcloud", "/perception/obstacle_segmentation/pointcloud"), + ("~/input/map", "/map/vector_map"), + ("~/output/debug_markers", "debug_markers"), + ("~/output/trajectory", "obstacle_velocity_limiter/trajectory"), + ], + parameters=[ + obstacle_velocity_limiter_param, + vehicle_info_param, + {"obstacles.dynamic_source": "static_only"}, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + # surround obstacle checker surround_obstacle_checker_param_path = os.path.join( LaunchConfiguration("tier4_planning_launch_param_path").perform(context), @@ -167,7 +201,7 @@ def launch_setup(context, *args, **kwargs): ), ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), ], parameters=[ nearest_search_param, @@ -197,7 +231,7 @@ def launch_setup(context, *args, **kwargs): name="obstacle_cruise_planner", namespace="", remappings=[ - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), + ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/acceleration", "/localization/acceleration"), ("~/input/objects", "/perception/object_recognition/objects"), @@ -220,7 +254,7 @@ def launch_setup(context, *args, **kwargs): name="obstacle_cruise_planner_relay", namespace="", parameters=[ - {"input_topic": "obstacle_avoidance_planner/trajectory"}, + {"input_topic": "obstacle_velocity_limiter/trajectory"}, {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"}, {"type": "autoware_auto_planning_msgs/msg/Trajectory"}, ], @@ -234,6 +268,7 @@ def launch_setup(context, *args, **kwargs): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ obstacle_avoidance_planner_component, + obstacle_velocity_limiter_component, ], ) diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 7033ab6aa87af..45e2e186336a1 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -67,6 +67,7 @@ obstacle_cruise_planner obstacle_stop_planner planning_error_monitor + rtc_auto_mode_manager scenario_selector surround_obstacle_checker diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 08d028a7f0d63..08a286c06c106 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -15,6 +15,7 @@ #ifndef EKF_LOCALIZER__EKF_LOCALIZER_HPP_ #define EKF_LOCALIZER__EKF_LOCALIZER_HPP_ +#include "ekf_localizer/hyper_parameters.hpp" #include "ekf_localizer/warning.hpp" #include @@ -162,39 +163,15 @@ class EKFLocalizer : public rclcpp::Node Simple1DFilter roll_filter_; Simple1DFilter pitch_filter_; + const HyperParameters params_; + + double ekf_rate_; + double ekf_dt_; + /* parameters */ - bool show_debug_info_; - double ekf_rate_; //!< @brief EKF predict rate - double ekf_dt_; //!< @brief = 1 / ekf_rate_ - double tf_rate_; //!< @brief tf publish rate - bool enable_yaw_bias_estimation_; //!< @brief for LiDAR mount error. - //!< if true,publish /estimate_yaw_bias - std::string pose_frame_id_; - - int dim_x_; //!< @brief dimension of EKF state - int extend_state_step_; //!< @brief for time delay compensation - int dim_x_ex_; //!< @brief dimension of extended EKF state (dim_x_ * extended_state_step) - /* Pose */ - double pose_additional_delay_; //!< @brief compensated pose delay time = - //!< (pose.header.stamp - now) + additional_delay [s] - double pose_measure_uncertainty_time_; //!< @brief added for measurement covariance - double pose_rate_; //!< @brief pose rate [s], used for covariance calculation - //!< @brief the mahalanobis distance threshold to ignore pose measurement - double pose_gate_dist_; - - /* twist */ - double twist_additional_delay_; //!< @brief compensated delay = (twist.header.stamp - now) - //!< + additional_delay [s] - double twist_rate_; //!< @brief rate [s], used for covariance calculation - //!< @brief measurement is ignored if the mahalanobis distance is larger than this value. - double twist_gate_dist_; - - /* process noise standard deviation */ - double proc_stddev_yaw_c_; //!< @brief yaw process noise - double proc_stddev_yaw_bias_c_; //!< @brief yaw bias process noise - double proc_stddev_vx_c_; //!< @brief vx process noise - double proc_stddev_wz_c_; //!< @brief wz process noise + int dim_x_; //!< @brief dimension of EKF state + int dim_x_ex_; //!< @brief dimension of extended EKF state (dim_x_ * extended_state_step) /* process noise variance for discrete model */ double proc_cov_yaw_d_; //!< @brief discrete yaw process noise @@ -214,9 +191,6 @@ class EKFLocalizer : public rclcpp::Node std::array current_pose_covariance_; std::array current_twist_covariance_; - int pose_smoothing_steps_; - int twist_smoothing_steps_; - /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp new file mode 100644 index 0000000000000..c74fa9be79525 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -0,0 +1,64 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ +#define EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ + +#include + +#include +#include + +class HyperParameters +{ +public: + explicit HyperParameters(rclcpp::Node * node) + : show_debug_info(node->declare_parameter("show_debug_info", false)), + ekf_rate(node->declare_parameter("predict_frequency", 50.0)), + ekf_dt(1.0 / std::max(ekf_rate, 0.1)), + tf_rate_(node->declare_parameter("tf_rate", 10.0)), + enable_yaw_bias_estimation(node->declare_parameter("enable_yaw_bias_estimation", true)), + extend_state_step(node->declare_parameter("extend_state_step", 50)), + pose_frame_id(node->declare_parameter("pose_frame_id", std::string("map"))), + pose_additional_delay(node->declare_parameter("pose_additional_delay", 0.0)), + pose_gate_dist(node->declare_parameter("pose_gate_dist", 10000.0)), + pose_smoothing_steps(node->declare_parameter("pose_smoothing_steps", 5)), + twist_additional_delay(node->declare_parameter("twist_additional_delay", 0.0)), + twist_gate_dist(node->declare_parameter("twist_gate_dist", 10000.0)), + twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)), + proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)), + proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)), + proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)) + { + } + + const bool show_debug_info; + const double ekf_rate; + const double ekf_dt; + const double tf_rate_; + const bool enable_yaw_bias_estimation; + const int extend_state_step; + const std::string pose_frame_id; + const double pose_additional_delay; + const double pose_gate_dist; + const int pose_smoothing_steps; + const double twist_additional_delay; + const double twist_gate_dist; + const int twist_smoothing_steps; + const double proc_stddev_vx_c; //!< @brief vx process noise + const double proc_stddev_wz_c; //!< @brief wz process noise + const double proc_stddev_yaw_c; //!< @brief yaw process noise +}; + +#endif // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 7c80f3616beca..da53e16afee56 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -39,44 +39,26 @@ // clang-format off #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) {if (show_debug_info_) {RCLCPP_INFO(__VA_ARGS__);}} -#define DEBUG_PRINT_MAT(X) {if (show_debug_info_) {std::cout << #X << ": " << X << std::endl;}} - +#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} +#define DEBUG_PRINT_MAT(X) {\ + if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\ +} // clang-format on using std::placeholders::_1; EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), warning_(this), dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) +: rclcpp::Node(node_name, node_options), + warning_(this), + params_(this), + ekf_rate_(params_.ekf_rate), + ekf_dt_(params_.ekf_dt), + dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */) { - show_debug_info_ = declare_parameter("show_debug_info", false); - ekf_rate_ = declare_parameter("predict_frequency", 50.0); - ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); - tf_rate_ = declare_parameter("tf_rate", 10.0); - enable_yaw_bias_estimation_ = declare_parameter("enable_yaw_bias_estimation", true); - extend_state_step_ = declare_parameter("extend_state_step", 50); - pose_frame_id_ = declare_parameter("pose_frame_id", std::string("map")); - - /* pose measurement */ - pose_additional_delay_ = declare_parameter("pose_additional_delay", 0.0); - pose_measure_uncertainty_time_ = declare_parameter("pose_measure_uncertainty_time", 0.01); - pose_gate_dist_ = declare_parameter("pose_gate_dist", 10000.0); // Mahalanobis limit - pose_smoothing_steps_ = declare_parameter("pose_smoothing_steps", 5); - - /* twist measurement */ - twist_additional_delay_ = declare_parameter("twist_additional_delay", 0.0); - twist_gate_dist_ = declare_parameter("twist_gate_dist", 10000.0); // Mahalanobis limit - twist_smoothing_steps_ = declare_parameter("twist_smoothing_steps", 2); - - /* process noise */ - proc_stddev_yaw_c_ = declare_parameter("proc_stddev_yaw_c", 0.005); - proc_stddev_vx_c_ = declare_parameter("proc_stddev_vx_c", 5.0); - proc_stddev_wz_c_ = declare_parameter("proc_stddev_wz_c", 1.0); - /* convert to continuous to discrete */ - proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0); - proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0); - proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); + proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0); + proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0); + proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0); is_activated_ = false; @@ -86,7 +68,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti std::bind(&EKFLocalizer::timerCallback, this)); timer_tf_ = rclcpp::create_timer( - this, get_clock(), rclcpp::Rate(tf_rate_).period(), + this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), std::bind(&EKFLocalizer::timerTFCallback, this)); pub_pose_ = create_publisher("ekf_pose", 1); @@ -112,7 +94,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti &EKFLocalizer::serviceTriggerNode, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile()); - dim_x_ex_ = dim_x_ * extend_state_step_; + dim_x_ex_ = dim_x_ * params_.extend_state_step; tf_br_ = std::make_shared( std::shared_ptr(this, [](auto) {})); @@ -142,9 +124,9 @@ void EKFLocalizer::updatePredictFrequency() ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); /* Update discrete proc_cov*/ - proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0); - proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0); - proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); + proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0); + proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0); + proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0); } } last_predict_time_ = std::make_shared(get_clock()->now()); @@ -224,7 +206,7 @@ void EKFLocalizer::timerCallback() const double vx = ekf_.getXelement(IDX::VX); const double wz = ekf_.getXelement(IDX::WZ); - current_ekf_pose_.header.frame_id = pose_frame_id_; + current_ekf_pose_.header.frame_id = params_.pose_frame_id; current_ekf_pose_.header.stamp = this->now(); current_ekf_pose_.pose.position.x = x; current_ekf_pose_.pose.position.y = y; @@ -247,7 +229,7 @@ void EKFLocalizer::timerCallback() void EKFLocalizer::showCurrentX() { - if (show_debug_info_) { + if (params_.show_debug_info) { const Eigen::MatrixXd X = ekf_.getLatestX(); DEBUG_PRINT_MAT(X.transpose()); } @@ -318,10 +300,10 @@ void EKFLocalizer::callbackInitialPose( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) { geometry_msgs::msg::TransformStamped transform; - if (!getTransformFromTF(pose_frame_id_, initialpose->header.frame_id, transform)) { + if (!getTransformFromTF(params_.pose_frame_id, initialpose->header.frame_id, transform)) { RCLCPP_ERROR( - get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(), - initialpose->header.frame_id.c_str()); + get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", + params_.pose_frame_id.c_str(), initialpose->header.frame_id.c_str()); } Eigen::MatrixXd X(dim_x_, 1); @@ -344,13 +326,14 @@ void EKFLocalizer::callbackInitialPose( P(IDX::Y, IDX::Y) = initialpose->pose.covariance[COV_IDX::Y_Y]; P(IDX::YAW, IDX::YAW) = initialpose->pose.covariance[COV_IDX::YAW_YAW]; - if (enable_yaw_bias_estimation_) { + if (params_.enable_yaw_bias_estimation) { P(IDX::YAWB, IDX::YAWB) = 0.0001; } P(IDX::VX, IDX::VX) = 0.01; P(IDX::WZ, IDX::WZ) = 0.01; - ekf_.init(X, P, extend_state_step_); + ekf_.init(X, P, params_.extend_state_step); + updateSimple1DFilters(*initialpose); } @@ -360,8 +343,11 @@ void EKFLocalizer::callbackInitialPose( void EKFLocalizer::callbackPoseWithCovariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - if (!is_activated_) return; - PoseInfo pose_info = {msg, 0, pose_smoothing_steps_}; + if (!is_activated_) { + return; + } + PoseInfo pose_info = {msg, 0, params_.pose_smoothing_steps}; + current_pose_info_queue_.push(pose_info); updateSimple1DFilters(*msg); @@ -373,7 +359,7 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - TwistInfo twist_info = {msg, 0, twist_smoothing_steps_}; + TwistInfo twist_info = {msg, 0, params_.twist_smoothing_steps}; current_twist_info_queue_.push(twist_info); } @@ -385,13 +371,13 @@ void EKFLocalizer::initEKF() Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y P(IDX::YAW, IDX::YAW) = 50.0; // for yaw - if (enable_yaw_bias_estimation_) { + if (params_.enable_yaw_bias_estimation) { P(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias } P(IDX::VX, IDX::VX) = 1000.0; // for vx P(IDX::WZ, IDX::WZ) = 50.0; // for wz - ekf_.init(X, P, extend_state_step_); + ekf_.init(X, P, params_.extend_state_step); } /* @@ -445,11 +431,11 @@ void EKFLocalizer::predictKinematicsModel() */ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { - if (pose.header.frame_id != pose_frame_id_) { + if (pose.header.frame_id != params_.pose_frame_id) { warning_.warnThrottle( fmt::format( "pose frame_id is %s, but pose_frame is set as %s. They must be same.", - pose.header.frame_id.c_str(), pose_frame_id_.c_str()), + pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), 2000); } const Eigen::MatrixXd X_curr = ekf_.getLatestX(); @@ -459,7 +445,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const rclcpp::Time t_curr = this->now(); /* Calculate delay step */ - double delay_time = (t_curr - pose.header.stamp).seconds() + pose_additional_delay_; + double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; if (delay_time < 0.0) { delay_time = 0.0; warning_.warnThrottle( @@ -467,12 +453,12 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar 1000); } int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > extend_state_step_ - 1) { + if (delay_step > params_.extend_state_step - 1) { warning_.warnThrottle( fmt::format( "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", - delay_time, extend_state_step_ * ekf_dt_), + delay_time, params_.extend_state_step * ekf_dt_), 1000); return; } @@ -500,7 +486,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw; const Eigen::MatrixXd P_curr = ekf_.getLatestP(); const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); - if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y)) { + if (!mahalanobisGate(params_.pose_gate_dist, y_ekf, y, P_y)) { warning_.warnThrottle( "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore " "measurement data.", @@ -513,7 +499,8 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar DEBUG_PRINT_MAT((y - y_ekf).transpose()); const Eigen::Matrix C = poseMeasurementMatrix(); - const Eigen::Matrix3d R = poseMeasurementCovariance(pose.pose.covariance, pose_smoothing_steps_); + const Eigen::Matrix3d R = + poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); ekf_.updateWithDelay(y, C, R, delay_step); @@ -542,7 +529,7 @@ void EKFLocalizer::measurementUpdateTwist( const rclcpp::Time t_curr = this->now(); /* Calculate delay step */ - double delay_time = (t_curr - twist.header.stamp).seconds() + twist_additional_delay_; + double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; if (delay_time < 0.0) { warning_.warnThrottle( fmt::format( @@ -551,12 +538,12 @@ void EKFLocalizer::measurementUpdateTwist( delay_time = 0.0; } int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > extend_state_step_ - 1) { + if (delay_step > params_.extend_state_step - 1) { warning_.warnThrottle( fmt::format( "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " "extend_state_step * ekf_dt : %f [s]", - delay_time, extend_state_step_ * ekf_dt_), + delay_time, params_.extend_state_step * ekf_dt_), 1000); return; } @@ -578,7 +565,7 @@ void EKFLocalizer::measurementUpdateTwist( ekf_.getXelement(delay_step * dim_x_ + IDX::WZ); const Eigen::MatrixXd P_curr = ekf_.getLatestP(); const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); - if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y)) { + if (!mahalanobisGate(params_.twist_gate_dist, y_ekf, y, P_y)) { warning_.warnThrottle( "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore " "measurement data.", @@ -592,7 +579,7 @@ void EKFLocalizer::measurementUpdateTwist( const Eigen::Matrix C = twistMeasurementMatrix(); const Eigen::Matrix2d R = - twistMeasurementCovariance(twist.twist.covariance, twist_smoothing_steps_); + twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps); ekf_.updateWithDelay(y, C, R, delay_step); 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 0dd3ae9015e7a..20548a1f21d38 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 @@ -66,15 +66,6 @@ enum class ConvergedParamType { NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1 }; -struct NdtResult -{ - geometry_msgs::msg::Pose pose; - float transform_probability; - float nearest_voxel_transformation_likelihood; - int iteration_num; - std::vector transformation_array; -}; - struct NDTParams { double trans_epsilon; @@ -111,7 +102,6 @@ class NDTScanMatcher : public rclcpp::Node void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - NdtResult align(const geometry_msgs::msg::Pose & initial_pose_msg); geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( const std::shared_ptr & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); 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 4c4e1beeb772f..48e958abe6a9f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -376,7 +376,7 @@ void NDTScanMatcher::callback_map_points( // create Thread // detach auto output_cloud = std::make_shared>(); - new_ndt_ptr->align(*output_cloud, Eigen::Matrix4f::Identity()); + new_ndt_ptr->align(*output_cloud); // swap ndt_ptr_mtx_.lock(); @@ -429,7 +429,11 @@ void NDTScanMatcher::callback_sensor_points( // perform ndt scan matching key_value_stdmap_["state"] = "Aligning"; - const NdtResult ndt_result = align(interpolator.get_current_pose().pose.pose); + const Eigen::Matrix4f initial_pose_matrix = + pose_to_matrix4f(interpolator.get_current_pose().pose.pose); + auto output_cloud = std::make_shared>(); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); key_value_stdmap_["state"] = "Sleeping"; const auto exe_end_time = std::chrono::system_clock::now(); @@ -437,6 +441,13 @@ void NDTScanMatcher::callback_sensor_points( std::chrono::duration_cast(exe_end_time - exe_start_time).count() / 1000.0; + const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose); + std::vector transformation_msg_array; + for (const auto & pose_matrix : ndt_result.transformation_array) { + geometry_msgs::msg::Pose pose_ros = matrix4f_to_pose(pose_matrix); + transformation_msg_array.push_back(pose_ros); + } + // perform several validations /***************************************************************************** The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in @@ -455,7 +466,7 @@ void NDTScanMatcher::callback_sensor_points( bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( - ndt_result.transformation_array, oscillation_threshold_, inversion_vector_threshold_); + transformation_msg_array, oscillation_threshold_, inversion_vector_threshold_); } bool is_ok_converged_param = validate_converged_param( ndt_result.transform_probability, ndt_result.nearest_voxel_transformation_likelihood); @@ -476,16 +487,16 @@ void NDTScanMatcher::callback_sensor_points( nearest_voxel_transformation_likelihood_pub_->publish( make_float32_stamped(sensor_ros_time, ndt_result.nearest_voxel_transformation_likelihood)); iteration_num_pub_->publish(make_int32_stamped(sensor_ros_time, ndt_result.iteration_num)); - publish_tf(sensor_ros_time, ndt_result.pose); - publish_pose(sensor_ros_time, ndt_result.pose, is_converged); - publish_marker(sensor_ros_time, ndt_result.transformation_array); + publish_tf(sensor_ros_time, result_pose_msg); + publish_pose(sensor_ros_time, result_pose_msg, is_converged); + publish_marker(sensor_ros_time, transformation_msg_array); publish_initial_to_result_distances( - sensor_ros_time, ndt_result.pose, interpolator.get_current_pose(), interpolator.get_old_pose(), + sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), interpolator.get_new_pose()); auto sensor_points_mapTF_ptr = std::make_shared>(); pcl::transformPointCloud( - *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, pose_to_matrix4f(ndt_result.pose)); + *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_mapTF_ptr); key_value_stdmap_["transform_probability"] = std::to_string(ndt_result.transform_probability); @@ -516,31 +527,6 @@ void NDTScanMatcher::transform_sensor_measurement( *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); } -NdtResult NDTScanMatcher::align(const geometry_msgs::msg::Pose & initial_pose_msg) -{ - const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose_msg); - - auto output_cloud = std::make_shared>(); - ndt_ptr_->align(*output_cloud, initial_pose_matrix); - - const std::vector> - transformation_array_matrix = ndt_ptr_->getFinalTransformationArray(); - std::vector transformation_array_msg; - for (auto pose_matrix : transformation_array_matrix) { - geometry_msgs::msg::Pose pose_ros = matrix4f_to_pose(pose_matrix); - transformation_array_msg.push_back(pose_ros); - } - - NdtResult ndt_result; - ndt_result.pose = matrix4f_to_pose(ndt_ptr_->getFinalTransformation()); - ndt_result.transformation_array = transformation_array_msg; - ndt_result.transform_probability = ndt_ptr_->getTransformationProbability(); - ndt_result.nearest_voxel_transformation_likelihood = - ndt_ptr_->getNearestVoxelTransformationLikelihood(); - ndt_result.iteration_num = ndt_ptr_->getFinalNumIteration(); - return ndt_result; -} - geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo( const std::shared_ptr & ndt_ptr, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) @@ -559,18 +545,20 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ for (unsigned int i = 0; i < initial_poses.size(); i++) { const auto & initial_pose = initial_poses[i]; - const NdtResult ndt_result = align(initial_pose); + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); Particle particle( - initial_pose, ndt_result.pose, ndt_result.transform_probability, ndt_result.iteration_num); + initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, + ndt_result.iteration_num); particle_array.push_back(particle); const auto marker_array = make_debug_markers( this->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, i); ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); auto sensor_points_mapTF_ptr = std::make_shared>(); - pcl::transformPointCloud( - *ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, pose_to_matrix4f(ndt_result.pose)); + pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index ad082fa84d42e..1da538089f46b 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -411,9 +411,9 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const const double left_offset = std::max( *itr.second + (*itr.first > threshold ? margin : 0.0), parameters_.drivable_area_left_bound_offset); - const double right_offset = std::min( + const double right_offset = -std::min( *itr.first - (*itr.first < -threshold ? margin : 0.0), - parameters_.drivable_area_right_bound_offset); + -parameters_.drivable_area_right_bound_offset); const auto extended_lanelets = util::expandLanelets(current_lanelets_, left_offset, right_offset); diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index a7519f58794d3..d40fb297a3a50 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include namespace behavior_velocity_planner @@ -42,6 +43,8 @@ int insertPoint( autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id); +std::optional> findLaneIdInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id); bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx); @@ -55,6 +58,7 @@ std::tuple getObjectiveLanelets( struct StopLineIdx { + // TODO(Mamoru Sobue): replace optional int first_idx_inside_lane = -1; int pass_judge_line_idx = -1; int stop_line_idx = -1; @@ -98,11 +102,14 @@ bool generateStopLineBeforeIntersection( /** * @brief Calculate first path index that is in the polygon. * @param path target path + * @param lane_interval_start the start index of point on the lane + * @param lane_interval_end the last index of point on the lane * @param polygons target polygon * @return path point index */ -int getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +std::optional getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start, + const size_t lane_interval_end, const int lane_id, const std::vector & polygons); /** @@ -111,7 +118,8 @@ int getFirstPointInsidePolygons( * @return true when the stop point is defined on map. */ bool getStopLineIndexFromMap( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_id, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start, + const size_t lane_interval_end, const int lane_id, const std::shared_ptr & planner_data, int * stop_idx_ip, int dist_thr, const rclcpp::Logger logger); 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 8b82c33b59d98..197fcd1bf9d38 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -74,6 +74,29 @@ bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, return false; } +std::optional> findLaneIdInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const int lane_id) +{ + bool found = false; + size_t start = 0; + size_t end = p.points.size() > 0 ? p.points.size() - 1 : 0; + for (size_t i = 0; i < p.points.size(); ++i) { + if (hasLaneId(p.points.at(i), lane_id)) { + if (!found) { + // found interval for the first time + found = true; + start = i; + } + } else if (found) { + // prior point was in the interval. interval ended + end = i; + break; + } + } + start = start > 0 ? start - 1 : 0; // the idx of last point before the interval + return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; +} + bool getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point, int * duplicated_point_idx) @@ -91,20 +114,20 @@ bool getDuplicatedPointIdx( return false; } -// TODO(Mamoru Sobue): return optional, not -1 -int getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +std::optional getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start, + const size_t lane_interval_end, [[maybe_unused]] const int lane_id, const std::vector & polygons) { - int first_idx_inside_lanelet = -1; - for (size_t i = 0; i < path.points.size(); ++i) { + std::optional first_idx_inside_lanelet = std::nullopt; + for (size_t i = lane_interval_start; i <= lane_interval_end; ++i) { bool is_in_lanelet = false; auto p = path.points.at(i).point.pose.position; for (const auto & polygon : polygons) { const auto polygon_2d = lanelet::utils::to2D(polygon); is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); if (is_in_lanelet) { - first_idx_inside_lanelet = static_cast(i); + first_idx_inside_lanelet = i; break; } } @@ -135,7 +158,6 @@ bool generateStopLine( /* spline interpolation */ constexpr double interval = 0.2; autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; - // TODO(Mamoru Sobue): crop only intersection part of path for computation cost if (!splineInterpolate(target_path, interval, path_ip, logger)) { return false; } @@ -157,25 +179,37 @@ bool generateStopLine( // stop_line_margin). // stop point index for interpolated(ip) path. int stop_idx_ip; - if (getStopLineIndexFromMap(path_ip, lane_id, planner_data, &stop_idx_ip, 10.0, logger)) { + const auto lane_interval_opt = util::findLaneIdInterval(path_ip, lane_id); + if (!lane_interval_opt.has_value()) { + RCLCPP_INFO(logger, "Path has no interval on intersection lane %d", lane_id); + return false; + } + const auto [lane_interval_start, lane_interval_end] = lane_interval_opt.value(); + if (getStopLineIndexFromMap( + path_ip, lane_interval_start, lane_interval_end, lane_id, planner_data, &stop_idx_ip, 10.0, + logger)) { stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); } else { // find the index of the first point that intersects with detection_areas - const int first_idx_ip_inside_lane = getFirstPointInsidePolygons(path_ip, detection_areas); + const auto first_idx_ip_inside_lane_opt = getFirstPointInsidePolygons( + path_ip, lane_interval_start, lane_interval_end, lane_id, detection_areas); // if path is not intersecting with detection_area, skip - if (first_idx_ip_inside_lane == -1) { + if (!first_idx_ip_inside_lane_opt.has_value()) { RCLCPP_DEBUG( logger, "Path is not intersecting with detection_area, not generating stop_line"); return false; } + const auto first_idx_ip_inside_lane = first_idx_ip_inside_lane_opt.value(); const auto & first_inside_point = path_ip.points.at(first_idx_ip_inside_lane).point.pose; const auto first_idx_inside_lane_opt = motion_utils::findNearestIndex(original_path->points, first_inside_point, 10.0, M_PI_4); if (first_idx_inside_lane_opt) { *first_idx_inside_lane = first_idx_inside_lane_opt.get(); } - stop_idx_ip = - std::max(first_idx_ip_inside_lane - 1 - stop_line_margin_idx_dist - base2front_idx_dist, 0); + stop_idx_ip = static_cast(std::max( + static_cast(first_idx_ip_inside_lane) - 1 - stop_line_margin_idx_dist - + base2front_idx_dist, + 0)); } if (stop_idx_ip == 0) { @@ -233,7 +267,8 @@ bool generateStopLine( } bool getStopLineIndexFromMap( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_id, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t lane_interval_start, + const size_t lane_interval_end, const int lane_id, const std::shared_ptr & planner_data, int * stop_idx_ip, int dist_thr, const rclcpp::Logger logger) { @@ -258,7 +293,7 @@ bool getStopLineIndexFromMap( const LineString2d extended_stop_line = planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length); - for (size_t i = 0; i < path.points.size() - 1; i++) { + for (size_t i = lane_interval_start; i <= lane_interval_end; i++) { const auto & p_front = path.points.at(i).point.pose.position; const auto & p_back = path.points.at(i + 1).point.pose.position; @@ -295,7 +330,6 @@ bool getStopLineIndexFromMap( return true; } -// TODO(Mamoru Sobue): return std::tuple std::tuple getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on) diff --git a/planning/behavior_velocity_planner/traffic-light-design.md b/planning/behavior_velocity_planner/traffic-light-design.md index 2c5921e555604..91be0cf41cfc5 100644 --- a/planning/behavior_velocity_planner/traffic-light-design.md +++ b/planning/behavior_velocity_planner/traffic-light-design.md @@ -71,7 +71,7 @@ This module is activated when there is traffic light in ego lane. | `tl_state_timeout` | double | [s] time out for detected traffic light result. | | `external_tl_state_timeout` | double | [s] time out for external traffic input | | `yellow_lamp_period` | double | [s] time for yellow lamp | -| `enable_pass_judge` | bool | [-] weather to use pass judge | +| `enable_pass_judge` | bool | [-] whether to use pass judge | #### Flowchart diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index a8008e5804b55..aa47a924a76f6 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1001,7 +1001,7 @@ bool ObstacleAvoidancePlanner::checkReplan(const PlannerData & planner_data) return true; } - if (isPathGoalChanged(planner_data)) { + if (isPathGoalChanged(p)) { RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed."); resetPrevOptimization(); return true; @@ -1077,7 +1077,7 @@ bool ObstacleAvoidancePlanner::isPathGoalChanged(const PlannerData & planner_dat { const auto & p = planner_data; - if (prev_path_points_ptr_) { + if (!prev_path_points_ptr_) { return false; } diff --git a/planning/obstacle_velocity_limiter/CMakeLists.txt b/planning/obstacle_velocity_limiter/CMakeLists.txt new file mode 100644 index 0000000000000..0f2363b25be45 --- /dev/null +++ b/planning/obstacle_velocity_limiter/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.5) +project(obstacle_velocity_limiter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED COMPONENTS common) +find_package(pcl_conversions REQUIRED) + +ament_auto_add_library(obstacle_velocity_limiter_node SHARED + DIRECTORY src +) + +ament_target_dependencies(obstacle_velocity_limiter_node PCL) +target_include_directories(obstacle_velocity_limiter_node + SYSTEM PUBLIC + "${PCL_INCLUDE_DIRS}" +) + +# Disable warnings due to external dependencies +get_target_property(lanelet2_core_INCLUDE_DIR + lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES +) + +rclcpp_components_register_node(obstacle_velocity_limiter_node + PLUGIN "obstacle_velocity_limiter::ObstacleVelocityLimiterNode" + EXECUTABLE obstacle_velocity_limiter +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_forward_projection.cpp + test/test_obstacles.cpp + test/test_collision_distance.cpp + test/test_occupancy_grid_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + obstacle_velocity_limiter_node + ) +endif() + +add_executable(collision_benchmark + benchmarks/collision_checker_benchmark.cpp +) +target_link_libraries(collision_benchmark + obstacle_velocity_limiter_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config + script +) diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md new file mode 100644 index 0000000000000..3a0807d4e1a5d --- /dev/null +++ b/planning/obstacle_velocity_limiter/README.md @@ -0,0 +1,209 @@ +# Obstacle Velocity Limiter + +## Purpose + +This node limits the velocity when driving in the direction of an obstacle. +For example, it allows to reduce the velocity when driving close to a guard rail in a curve. + +| Without this node | With this node | +| ----------------------------------------------------: | :-------------------------------------------------- | +| ![obstacle_velocity_limiter_off](./media/ovl_off.png) | ![obstacle_velocity_limiter_on](./media/ovl_on.png) | + +## Inner-workings / Algorithms + +Using a parameter `min_ttc` (minimum time to collision), the node set velocity limits such that +no collision with an obstacle would occur, even without new control inputs for a duration of `min_ttc`. + +To achieve this, the motion of the ego vehicle is simulated forward in time at each point of the trajectory to create a corresponding footprint. +If the footprint collides with some obstacle, the velocity at the trajectory point is reduced such that the new simulated footprint do not have any collision. + +### Simulated Motion, Footprint, and Collision Distance + +The motion of the ego vehicle is simulated at each trajectory point using the `heading`, `velocity`, and `steering` defined at the point. +Footprints are then constructed from these simulations and checked for collision. +If a collision is found, the distance from the trajectory point is used to calculate the adjusted velocity that would produce a collision-free footprint. Parameter `simulation.distance_method` allow to switch between an exact distance calculation and a less expensive approximation using a simple euclidian distance. + +Two models can be selected with parameter `simulation.model` for simulating the motion of the vehicle: a simple particle model and a more complicated bicycle model. + +#### Particle Model + +The particle model uses the constant heading and velocity of the vehicle at a trajectory point to simulate the future motion. +The simulated forward motion corresponds to a straight line and the footprint to a rectangle. + +##### Footprint + +The rectangle footprint is built from 2 lines parallel to the simulated forward motion and at a distance of half the vehicle width. + +![particle_footprint_image](./media/particle_footprint.png) + +##### Distance + +When a collision point is found within the footprint, the distance is calculated as described in the following figure. + +![particle_collision_distance_image](./media/particle_distance.png) + +#### Bicycle Model + +The bicycle model uses the constant heading, velocity, and steering of the vehicle at a trajectory point to simulate the future motion. +The simulated forward motion corresponds to an arc around the circle of curvature associated with the steering. +Uncertainty in the steering can be introduced with the `simulation.steering_offset` parameter which will generate a range of motion from a left-most to a right-most steering. +This results in 3 curved lines starting from the same trajectory point. +A parameter `simulation.nb_points` is used to adjust the precision of these lines, with a minimum of `2` resulting in straight lines and higher values increasing the precision of the curves. + +By default, the steering values contained in the trajectory message are used. +Parameter `trajectory_preprocessing.calculate_steering_angles` allows to recalculate these values when set to `true`. + +##### Footprint + +The footprint of the bicycle model is created from lines parallel to the left and right simulated motion at a distance of half the vehicle width. +In addition, the two points on the left and right of the end point of the central simulated motion are used to complete the polygon. + +![bicycle_footprint_image](./media/bicycle_footprint.png) + +##### Distance + +The distance to a collision point is calculated by finding the curvature circle passing through the trajectory point and the collision point. + +![bicycle_collision_distance_image](./media/bicycle_distance.png) + +### Obstacle Detection + +Obstacles are represented as points or linestrings (i.e., sequence of points) around the obstacles and are constructed from an occupancy grid, a pointcloud, or the lanelet map. +The lanelet map is always checked for obstacles but the other source is switched using parameter `obstacles.dynamic_source`. + +To efficiently find obstacles intersecting with a footprint, they are stored in a [R-tree](https://www.boost.org/doc/libs/1_80_0/libs/geometry/doc/html/geometry/reference/spatial_indexes/boost__geometry__index__rtree.html). +Two trees are used, one for the obstacle points, and one for the obstacle linestrings (which are decomposed into segments to simplify the R-tree). + +#### Obstacle masks + +##### Dynamic obstacles + +Moving obstacles such as other cars should not be considered by this module. +These obstacles are detected by the perception modules and represented as polygons. +Obstacles inside these polygons are ignored. + +Only dynamic obstacles with a velocity above parameter `obstacles.dynamic_obstacles_min_vel` are removed. + +To deal with delays and precision errors, the polygons can be enlarged with parameter `obstacles.dynamic_obstacles_buffer`. + +##### Obstacles outside of the safety envelope + +Obstacles that are not inside any forward simulated footprint are ignored if parameter `obstacles.filter_envelope` is set to true. +The safety envelope polygon is built from all the footprints and used as a positive mask on the occupancy grid or pointcloud. + +This option can reduce the total number of obstacles which reduces the cost of collision detection. +However, the cost of masking the envelope is usually too high to be interesting. + +##### Obstacles on the ego path + +If parameter `obstacles.ignore_obstacles_on_path` is set to `true`, a polygon mask is built from the trajectory and the vehicle dimension. Any obstacle in this polygon is ignored. + +The size of the polygon can be increased using parameter `obstacles.ignore_extra_distance` which is added to the vehicle lateral offset. + +This option is a bit expensive and should only be used in case of noisy dynamic obstacles where obstacles are wrongly detected on the ego path, causing unwanted velocity limits. + +#### Lanelet Map + +Information about static obstacles can be stored in the Lanelet map using the value of the `type` tag of linestrings. +If any linestring has a `type` with one of the value from parameter `obstacles.static_map_tags`, then it will be used as an obstacle. + +Obstacles from the lanelet map are not impacted by the masks. + +#### Occupancy Grid + +Masking is performed by iterating through the cells inside each polygon mask using the [`grid_map_utils::PolygonIterator`](https://github.com/autowarefoundation/autoware.universe/tree/main/common/grid_map_utils) function. +A threshold is then applied to only keep cells with an occupancy value above parameter `obstacles.occupancy_grid_threshold`. +Finally, the image is converted to an image and obstacle linestrings are extracted using the opencv function +[`findContour`](https://docs.opencv.org/3.4/d3/dc0/group__imgproc__shape.html#ga17ed9f5d79ae97bd4c7cf18403e1689a). + +#### Pointcloud + +Masking is performed using the [`pcl::CropHull`](https://pointclouds.org/documentation/classpcl_1_1_crop_hull.html) function. +Points from the pointcloud are then directly used as obstacles. + +### Velocity Adjustment + +If a collision is found, the velocity at the trajectory point is adjusted such that the resulting footprint would no longer collide with an obstacle: +$velocity = \frac{dist\_to\_collision}{min\_ttc}$ + +To prevent sudden deceleration of the ego vehicle, the parameter `max_deceleration` limits the deceleration relative to the current ego velocity. +For a trajectory point occuring at a duration `t` in the future (calculated from the original velocity profile), +the adjusted velocity cannot be set lower than $v_{current} - t * max\_deceleration$. + +Furthermore, a parameter `min_adjusted_velocity` +provides a lower bound on the modified velocity. + +### Trajectory preprocessing + +The node only modifies part of the input trajectory, starting from the current ego position. +Parameter `trajectory_preprocessing.start_distance` is used to adjust how far ahead of the ego position the velocities will start being modified. +Parameters `trajectory_preprocessing.max_length` and `trajectory_preprocessing.max_duration` are used to control how much of the trajectory will see its velocity adjusted. + +To reduce computation cost at the cost of precision, the trajectory can be downsampled using parameter `trajectory_preprocessing.downsample_factor`. +For example a value of `1` means all trajectory points will be evaluated while a value of `10` means only 1/10th of the points will be evaluated. + +## Inputs / Outputs + +### Inputs + +| Name | Type | Description | +| ----------------------------- | ------------------------------------------------ | -------------------------------------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | +| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | +| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | +| `~/input/dynamic_obstacles` | `autoware_auto_perception_msgs/PredictedObjects` | Dynamic objects | +| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | +| `~/input/map` | `autoware_auto_mapping_msgs/HADMapBin` | Vector map used to retrieve static obstacles | + +### Outputs + +| Name | Type | Description | +| ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | +| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | +| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | +| `~/output/runtime_microseconds` | `std_msgs/Int64` | Time taken to calculate the trajectory (in microseconds) | + +## Parameters + +| Name | Type | Description | +| --------------------------------------------------- | ----------- | --------------------------------------------------------------------------------------------------------------------------------------- | +| `min_ttc` | float | [s] required minimum time with no collision at each point of the trajectory assuming constant heading and velocity. | +| `distance_buffer` | float | [m] required distance buffer with the obstacles. | +| `min_adjusted_velocity` | float | [m/s] minimum adjusted velocity this node can set. | +| `max_deceleration` | float | [m/s²] maximum deceleration an adjusted velocity can cause. | +| `trajectory_preprocessing.start_distance` | float | [m] controls from which part of the trajectory (relative to the current ego pose) the velocity is adjusted. | +| `trajectory_preprocessing.max_length` | float | [m] controls the maximum length (starting from the `start_distance`) where the velocity is adjusted. | +| `trajectory_preprocessing.max_distance` | float | [s] controls the maximum duration (measured from the `start_distance`) where the velocity is adjusted. | +| `trajectory_preprocessing.downsample_factor` | int | trajectory downsampling factor to allow tradeoff between precision and performance. | +| `trajectory_preprocessing.calculate_steering_angle` | bool | if true, the steering angles of the trajectory message are not used but are recalculated. | +| `simulation.model` | string | model to use for forward simulation. Either "particle" or "bicycle". | +| `simulation.distance_method` | string | method to use for calculating distance to collision. Either "exact" or "approximation". | +| `simulation.steering_offset` | float | offset around the steering used by the bicycle model. | +| `simulation.nb_points` | int | number of points used to simulate motion with the bicycle model. | +| `obstacles.dynamic_source` | string | source of dynamic obstacle used for collision checking. Can be "occupancy_grid", "point_cloud", or "static_only" (no dynamic obstacle). | +| `obstacles.occupancy_grid_threshold` | int | value in the occupancy grid above which a cell is considered an obstacle. | +| `obstacles.dynamic_obstacles_buffer` | float | buffer around dynamic obstacles used when masking an obstacle in order to prevent noise. | +| `obstacles.dynamic_obstacles_min_vel` | float | velocity above which to mask a dynamic obstacle. | +| `obstacles.static_map_tags` | string list | linestring of the lanelet map with this tags are used as obstacles. | +| `obstacles.filter_envelope` | bool | wether to use the safety envelope to filter the dynamic obstacles source. | + +## Assumptions / Known limits + +The velocity profile produced by this node is not meant to be a realistic velocity profile +and can contain sudden jumps of velocity with no regard for acceleration and jerk. +This velocity profile is meant to be used as an upper bound on the actual velocity of the vehicle. + +## (Optional) Error detection and handling + +The critical case for this node is when an obstacle is falsely detected very close to the trajectory such that +the corresponding velocity suddenly becomes very low. +This can cause a sudden brake and two mechanisms can be used to mitigate these errors. + +Parameter `min_adjusted_velocity` allow to set a minimum to the adjusted velocity, preventing the node to slow down the vehicle too much. +Parameter `max_deceleration` allow to set a maximum deceleration (relative to the _current_ ego velocity) that the adjusted velocity would incur. + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp new file mode 100644 index 0000000000000..936476c800117 --- /dev/null +++ b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp @@ -0,0 +1,96 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include +#include + +using obstacle_velocity_limiter::CollisionChecker; +using obstacle_velocity_limiter::linestring_t; +using obstacle_velocity_limiter::Obstacles; +using obstacle_velocity_limiter::point_t; +using obstacle_velocity_limiter::polygon_t; + +point_t random_point() +{ + static std::random_device r; + static std::default_random_engine e1(r()); + static std::uniform_real_distribution uniform_dist(-100, 100); + return {uniform_dist(e1), uniform_dist(e1)}; +} + +linestring_t random_line() +{ + const auto point = random_point(); + const auto point2 = point_t{point.x() + 1, point.y() + 1}; + const auto point3 = point_t{point2.x() - 1, point2.y() + 1}; + const auto point4 = point_t{point3.x() + 1, point3.y() + 1}; + return {point, point2, point3, point4}; +} + +polygon_t random_polygon() +{ + polygon_t polygon; + const auto point = random_point(); + const auto point2 = point_t{point.x() + 1, point.y() + 4}; + const auto point3 = point_t{point.x() + 4, point.y() + 4}; + const auto point4 = point_t{point.x() + 3, point.y() + 1}; + polygon.outer() = {point, point2, point3, point4, point}; + return polygon; +} + +int main() +{ + Obstacles obstacles; + std::vector polygons; + polygons.reserve(100); + for (auto i = 0; i < 100; ++i) { + polygons.push_back(random_polygon()); + } + for (auto nb_lines = 1lu; nb_lines < 1000lu; nb_lines += 10) { + obstacles.lines.push_back(random_line()); + obstacles.points.clear(); + for (auto nb_points = 1lu; nb_points < 1000lu; nb_points += 10) { + obstacles.points.push_back(random_point()); + const auto rtt_constr_start = std::chrono::system_clock::now(); + CollisionChecker rtree_collision_checker(obstacles, 0, 0); + const auto rtt_constr_end = std::chrono::system_clock::now(); + const auto naive_constr_start = std::chrono::system_clock::now(); + CollisionChecker naive_collision_checker(obstacles, nb_points + 1, nb_lines * 100); + const auto naive_constr_end = std::chrono::system_clock::now(); + const auto rtt_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + const auto rtree_result = rtree_collision_checker.intersections(polygon); + const auto rtt_check_end = std::chrono::system_clock::now(); + const auto naive_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + const auto naive_result = naive_collision_checker.intersections(polygon); + const auto naive_check_end = std::chrono::system_clock::now(); + const auto rtt_constr_time = + std::chrono::duration_cast(rtt_constr_end - rtt_constr_start); + const auto naive_constr_time = + std::chrono::duration_cast(naive_constr_end - naive_constr_start); + const auto rtt_check_time = + std::chrono::duration_cast(rtt_check_end - rtt_check_start); + const auto naive_check_time = + std::chrono::duration_cast(naive_check_end - naive_check_start); + std::printf( + "%lu, %lu, %ld, %ld, %ld, %ld\n", nb_lines, nb_points, rtt_constr_time.count(), + rtt_check_time.count(), naive_constr_time.count(), naive_check_time.count()); + } + } + return 0; +} diff --git a/planning/obstacle_velocity_limiter/config/default_obstacle_velocity_limiter.param.yaml b/planning/obstacle_velocity_limiter/config/default_obstacle_velocity_limiter.param.yaml new file mode 100644 index 0000000000000..c6a0c275db0b4 --- /dev/null +++ b/planning/obstacle_velocity_limiter/config/default_obstacle_velocity_limiter.param.yaml @@ -0,0 +1,33 @@ +/**: + ros__parameters: + min_ttc: 1.0 # [s] minimum time without collision when doing projection at each trajectory point + distance_buffer: 0.0 # [m] extra distance to add to a projection (in addition to the vehicle overhang) + min_adjusted_velocity: 2.5 # [m/s] minimum velocity that the module can set + max_deceleration: 2.0 # [m/s²] maximum deceleration caused by the adjusted velocity + + trajectory_preprocessing: + start_distance: 0.0 # [m] distance ahead of ego from which to start modifying the trajectory + max_length: 100.0 # [m] maximum distance (from the start point) where the velocity is adjusted + max_duration: 10.0 # [s] maximum duration (from the start point) where the velocity is adjusted + downsample_factor: 10 # factor by which to downsample the input trajectory for calculation + calculate_steering_angles: false # if true, the steering angle at each trajectory point is calculated from the change in heading + + simulation: + model: particle # model to use for forward projection at each trajectory point. Either "particle" or "bicycle" + distance_method: exact # distance calculation method. Either "exact" or "approximation". + # parameters used only with the bicycle model + steering_offset: 0.01 # [rad] steering angle offset used to model uncertainty in the forward projection + nb_points: 5 # number of points representing the curved projections + + obstacles: + dynamic_source: static_only # source of dynamic obstacles. Must be 'pointcloud', 'occupancy_grid', or 'static_only'. + ignore_obstacles_on_path: false # if true, obstacles on the ego paths are ignored + ignore_extra_distance: 1.0 # [m] extra lateral distance where obstacles along the path are ignored + occupancy_grid_threshold: 60 # occupancy grid values higher than this are considered to be obstacles + dynamic_obstacles_buffer: 1.5 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection + dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module + static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles + - guard_rail + filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles + rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection + rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp new file mode 100644 index 0000000000000..7b9553a936ee7 --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp @@ -0,0 +1,51 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__DEBUG_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__DEBUG_HPP_ + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ +/// @brief make the visualization Marker of the given linestring +/// @param[in] ls linestring to turn into a marker +/// @param[in] z z-value to use in the marker +/// @return marker representing the linestring +visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, const Float z); + +/// @brief make debug marker array +/// @param[in] obstacles obstacles +/// @param[in] original_projections original forward projection linestrings +/// @param[in] adjusted_projections adjusted forward projection linestrings +/// @param[in] original_footprints original forward projection footprints +/// @param[in] adjusted_footprints adjusted forward projection footprints +/// @param[in] marker_z z-value to use for markers +/// @return marker array +visualization_msgs::msg::MarkerArray makeDebugMarkers( + const Obstacles & obstacles, const std::vector & original_projections, + const std::vector & adjusted_projections, + const std::vector & original_footprints, + const std::vector & adjusted_footprints, const ObstacleMasks & obstacle_masks, + const Float marker_z); + +} // namespace obstacle_velocity_limiter +#endif // OBSTACLE_VELOCITY_LIMITER__DEBUG_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp new file mode 100644 index 0000000000000..22d70463fc0d9 --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp @@ -0,0 +1,48 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__DISTANCE_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__DISTANCE_HPP_ + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/parameters.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ +/// @brief calculate the closest distance to a collision +/// @param [in] projection forward projection line +/// @param [in] footprint footprint of the projection +/// @param [in] collision_checker object to retrieve collision points +/// @param [in] params projection parameters +/// @return distance to the closest collision if any +std::optional distanceToClosestCollision( + const linestring_t & projection, const polygon_t & footprint, + const CollisionChecker & collision_checker, const ProjectionParameters & params); + +/// @brief calculate the closest distance along a circle to a given target point +/// @param [in] origin starting point +/// @param [in] heading heading used to calculate the tangent to the circle at the origin +/// @param [in] target target point +/// @return distance from origin to target along a circle +double arcDistance(const point_t & origin, const double heading, const point_t & target); + +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__DISTANCE_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp new file mode 100644 index 0000000000000..9c545e6fe2821 --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp @@ -0,0 +1,79 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__FORWARD_PROJECTION_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__FORWARD_PROJECTION_HPP_ + +#include "obstacle_velocity_limiter/parameters.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include + +namespace obstacle_velocity_limiter +{ +/// @brief generate a segment to where ego would reach with constant velocity and heading +/// @param [in] origin origin of the segment +/// @param [in] params parameters of the forward projection +/// @return segment from the origin to its position after duration + the extra_distance +segment_t forwardSimulatedSegment( + const geometry_msgs::msg::Point & origin, const ProjectionParameters & params); + +/// @brief generate lines for the forward projection using the bicycle model +/// @param [in] origin origin of the projection +/// @param [in] params parameters of the forward projection +/// @return lines from the origin to its positions after forward projection, ordered left to right +multilinestring_t bicycleProjectionLines( + const geometry_msgs::msg::Point & origin, const ProjectionParameters & params); + +/// @brief generate projection line using the bicycle model +/// @param [in] origin origin of the projection +/// @param [in] params parameters of the forward projection +/// @param [in] steering_angle steering angle used for the projection +/// @return line from the origin to its position after forward projection +linestring_t bicycleProjectionLine( + const geometry_msgs::msg::Point & origin, const ProjectionParameters & params, + const double steering_angle); + +/// @brief generate a footprint from a segment and a lateral offset +/// @param [in] segment segment from which to create the footprint +/// @param [in] lateral_offset offset around the segment used to create the footprint +/// @return footprint polygon +polygon_t generateFootprint(const segment_t & segment, const double lateral_offset); + +/// @brief generate a footprint from a linestring and a lateral offset +/// @param [in] linestring linestring from which to create the footprint +/// @param [in] lateral_offset offset around the segment used to create the footprint +/// @return footprint polygon +polygon_t generateFootprint(const linestring_t & linestring, const double lateral_offset); + +/// @brief generate a footprint from multiple linestrings and a lateral offset +/// @param [in] lines linestring from which to create the footprint +/// @param [in] lateral_offset offset around the segment used to create the footprint +/// @return footprint polygon +polygon_t generateFootprint(const multilinestring_t & lines, const double lateral_offset); + +/// @brief generate a footprint from a left, right, and middle linestrings and a lateral offset +/// @param [in] left left linestring +/// @param [in] middle left linestring +/// @param [in] right left linestring +/// @param [in] lateral_offset offset around the segment used to create the footprint +/// @return footprint polygon +polygon_t generateFootprint( + const linestring_t & left, const linestring_t & middle, const linestring_t & right, + const double lateral_offset); +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__FORWARD_PROJECTION_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp new file mode 100644 index 0000000000000..b1e8f848acd88 --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp @@ -0,0 +1,42 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__MAP_UTILS_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__MAP_UTILS_HPP_ + +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ + +/// @brief Extract static obstacles from the lanelet map +/// @param[in] lanelet_map lanelet map +/// @param[in] tags tags to identify obstacle linestrings +/// @return the extracted obstacles +multilinestring_t extractStaticObstacles( + const lanelet::LaneletMap & lanelet_map, const std::vector & tags); + +/// @brief Determine if the given linestring is an obstacle +/// @param[in] ls linestring to check +/// @param[in] tags obstacle tags +/// @return true if the linestring is an obstacle +bool isObstacle(const lanelet::ConstLineString3d & ls, const std::vector & tags); +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__MAP_UTILS_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp new file mode 100644 index 0000000000000..b95d3849cda9a --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -0,0 +1,132 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__OBSTACLE_VELOCITY_LIMITER_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__OBSTACLE_VELOCITY_LIMITER_HPP_ + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/parameters.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ + +/// @brief calculate the apparent safe velocity +/// @param[in] trajectory_point trajectory point for which to calculate the apparent safe velocity +/// @param[in] dist_to_collision distance from the trajectory point to the apparent collision +/// @return apparent safe velocity +Float calculateSafeVelocity( + const TrajectoryPoint & trajectory_point, const Float dist_to_collision); + +/// @brief calculate trajectory index that is ahead of the given index by the given distance +/// @param[in] trajectory trajectory +/// @param[in] ego_idx index closest to the current ego position in the trajectory +/// @param[in] start_distance desired distance ahead of the ego_idx +/// @return trajectory index ahead of ego_idx by the start_distance +size_t calculateStartIndex( + const Trajectory & trajectory, const size_t ego_idx, const Float start_distance); + +/// @brief downsample a trajectory, reducing its number of points by the given factor +/// @param[in] trajectory input trajectory +/// @param[in] start_idx starting index of the input trajectory +/// @param[in] factor factor used for downsampling +/// @return downsampled trajectory +Trajectory downsampleTrajectory( + const Trajectory & trajectory, const size_t start_idx, const int factor); + +/// @brief recalculate the steering angle of the trajectory +/// @details uses the change in headings for calculation +/// @param[inout] trajectory input trajectory +/// @param[in] wheel_base wheel base of the vehicle +void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); + +/// @brief create negative polygon masks from the dynamic objects +/// @param[in] dynamic_obstacles the dynamic objects to mask +/// @param[in] buffer buffer used to enlarge the mask +/// @param[in] min_vel minimum velocity for an object to be masked +/// @return polygon masks around dynamic objects +multipolygon_t createPolygonMasks( + const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, + const Float buffer, const Float min_vel); + +/// @brief create footprint polygons from projection lines +/// @details A footprint is create for each group of lines. Each group of lines is assumed to share +/// some points such that their footprint is a single connected polygon. +/// @param[in] projections the projection lines +/// @param[in] lateral_offset offset to create polygons around the lines +/// @return polygon footprint of each projection lines +std::vector createFootprintPolygons( + const std::vector & projected_linestrings, const Float lateral_offset); + +/// @brief create the footprint polygon from a trajectory +/// @param[in] trajectory the trajectory for which to create a footprint +/// @param[in] lateral_offset offset to create polygons around the trajectory points +/// @return polygon footprint of the trajectory +polygon_t createTrajectoryFootprint(const Trajectory & trajectory, const Float lateral_offset); + +/// @brief create a polygon of the safety envelope +/// @details the safety envelope is the area covered by forward projections at each trajectory +/// point +/// @param[in] trajectory input trajectory +/// @param[in] start_idx starting index in the input trajectory +/// @param[in] projection_params parameters of the forward projection +/// @return the envelope polygon +polygon_t createEnvelopePolygon( + const Trajectory & trajectory, const size_t start_idx, ProjectionParameters & projection_params); + +/// @brief create a polygon of the safety envelope from the projection footprints +/// @details the safety envelope is the area covered by forward projections at each trajectory +/// point +/// @param[in] footprints projection footprints +/// @return the envelope polygon +polygon_t createEnvelopePolygon(const std::vector & footprints); + +/// @brief create projection lines for each trajectory point +/// @details depending on the method used, multiple lines can be created for a same trajectory point +/// @param[in] trajectory input trajectory +/// @param[in] params projection parameters +/// @return projecton lines for each trajectory point +std::vector createProjectedLines( + const Trajectory & trajectory, ProjectionParameters & params); + +/// @brief limit the velocity of the given trajectory +/// @param[in] trajectory input trajectory +/// @param[in] collision_checker object used to retrive collision points +/// @param[in] projections forward projection lines at each trajectory point +/// @param[in] footprints footprint of the forward projection at each trajectory point +/// @param[in] projection_params projection parameters +/// @param[in] velocity_params velocity parameters +void limitVelocity( + Trajectory & trajectory, const CollisionChecker & collision_checker, + const std::vector & projections, const std::vector & footprints, + ProjectionParameters & projection_params, const VelocityParameters & velocity_params); + +/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory +/// @param[in] downsampled_trajectory downsampled trajectory +/// @param[in] trajectory input trajectory +/// @param[in] start_idx starting index of the downsampled trajectory relative to the input +/// @param[in] factor downsampling factor +/// @return input trajectory with the velocity profile of the downsampled trajectory +Trajectory copyDownsampledVelocity( + const Trajectory & downsampled_traj, Trajectory trajectory, const size_t start_idx, + const int factor); +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__OBSTACLE_VELOCITY_LIMITER_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp new file mode 100644 index 0000000000000..6ae31f98d0202 --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -0,0 +1,108 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__OBSTACLE_VELOCITY_LIMITER_NODE_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__OBSTACLE_VELOCITY_LIMITER_NODE_HPP_ + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/parameters.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace obstacle_velocity_limiter +{ + +class ObstacleVelocityLimiterNode : public rclcpp::Node +{ +public: + explicit ObstacleVelocityLimiterNode(const rclcpp::NodeOptions & node_options); + +private: + tier4_autoware_utils::TransformListener transform_listener_{this}; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; + rclcpp::Publisher::SharedPtr + pub_trajectory_; //!< @brief publisher for output trajectory + rclcpp::Publisher::SharedPtr + pub_debug_markers_; //!< @brief publisher for debug markers + rclcpp::Publisher::SharedPtr + pub_runtime_; //!< @brief publisher for callback runtime + rclcpp::Subscription::SharedPtr + sub_trajectory_; //!< @brief subscriber for reference trajectory + rclcpp::Subscription::SharedPtr + sub_objects_; //!< @brief subscribe for dynamic objects + rclcpp::Subscription::SharedPtr + sub_occupancy_grid_; //!< @brief subscriber for occupancy grid + rclcpp::Subscription::SharedPtr + sub_pointcloud_; //!< @brief subscriber for obstacle pointcloud + rclcpp::Subscription::SharedPtr + sub_odom_; //!< @brief subscriber for the current velocity + rclcpp::Subscription::SharedPtr map_sub_; + + // cached inputs + PredictedObjects::ConstSharedPtr dynamic_obstacles_ptr_; + OccupancyGrid::ConstSharedPtr occupancy_grid_ptr_; + PointCloud::ConstSharedPtr pointcloud_ptr_; + lanelet::LaneletMapPtr lanelet_map_ptr_{new lanelet::LaneletMap}; + multilinestring_t static_map_obstacles_; + std::optional current_ego_velocity_; + + // parameters + PreprocessingParameters preprocessing_params_; + ProjectionParameters projection_params_; + ObstacleParameters obstacle_params_; + VelocityParameters velocity_params_; + Float distance_buffer_ = static_cast(declare_parameter("distance_buffer")); + Float vehicle_lateral_offset_; + Float vehicle_front_offset_; + + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /// @brief callback for parameter updates + /// @param[in] parameters updated parameters and their new values + /// @return result of parameter update + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + /// @brief callback for input trajectories. Publishes a trajectory with updated velocities + /// @param[in] msg input trajectory message + void onTrajectory(const Trajectory::ConstSharedPtr msg); + + /// @brief validate the inputs of the node + /// @param[in] ego_idx trajectory index closest to the current ego pose + /// @return true if the inputs are valid + bool validInputs(const std::experimental::fundamentals_v1::optional & ego_idx); +}; +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__OBSTACLE_VELOCITY_LIMITER_NODE_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp new file mode 100644 index 0000000000000..4d82f31dc019c --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -0,0 +1,188 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__OBSTACLES_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__OBSTACLES_HPP_ + +#include "obstacle_velocity_limiter/parameters.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace obstacle_velocity_limiter +{ + +struct Obstacles +{ + multilinestring_t lines; + multipoint_t points; +}; + +namespace bgi = boost::geometry::index; +template +struct ObstacleTree +{ + explicit ObstacleTree(const T & obstacles); + [[nodiscard]] std::vector intersections(const polygon_t & polygon) const; +}; +template <> +struct ObstacleTree +{ + bgi::rtree> points_rtree; + + explicit ObstacleTree(const multipoint_t & obstacles) : points_rtree(obstacles) {} + + [[nodiscard]] std::vector intersections(const polygon_t & polygon) const + { + std::vector result; + // Query the points + points_rtree.query(bgi::covered_by(polygon), std::back_inserter(result)); + return result; + } +}; + +template <> +struct ObstacleTree +{ + bgi::rtree> segments_rtree; + + explicit ObstacleTree(const multilinestring_t & obstacles) + { + auto segment_count = 0lu; + for (const auto & line : obstacles) + if (!line.empty()) segment_count += line.size() - 1; + std::vector segments; + segments.reserve(segment_count); + for (const auto & line : obstacles) + for (size_t i = 0; i + 1 < line.size(); ++i) segments.emplace_back(line[i], line[i + 1]); + segments_rtree = bgi::rtree>(segments); + } + + [[nodiscard]] std::vector intersections(const polygon_t & polygon) const + { + std::vector result; + // Query the segments + std::vector candidates; + segments_rtree.query(bgi::intersects(polygon), std::back_inserter(candidates)); + std::vector intersection_points; + for (const auto & candidate : candidates) { + // need conversion to a linestring to use the 'intersection' and 'within' functions + const auto ls = linestring_t{candidate.first, candidate.second}; + intersection_points.clear(); + boost::geometry::intersection(ls, polygon, intersection_points); + if (intersection_points.empty()) { + // No intersection with the polygon: segment is outside or inside of the polygon + if (boost::geometry::within(ls, polygon)) { + result.push_back(candidate.first); + result.push_back(candidate.second); + } + } else { + result.insert(result.end(), intersection_points.begin(), intersection_points.end()); + } + } + return result; + } +}; + +struct CollisionChecker +{ + const Obstacles obstacles; + std::unique_ptr> point_obstacle_tree_ptr; + std::unique_ptr> line_obstacle_tree_ptr; + + explicit CollisionChecker( + Obstacles obs, const size_t rtree_min_points, const size_t rtree_min_segments) + : obstacles(std::move(obs)) + { + auto segment_count = 0lu; + for (const auto & line : obstacles.lines) + if (!line.empty()) segment_count += line.size() - 1; + if (segment_count > rtree_min_segments) + line_obstacle_tree_ptr = std::make_unique>(obstacles.lines); + if (obstacles.points.size() > rtree_min_points) + point_obstacle_tree_ptr = std::make_unique>(obstacles.points); + } + + [[nodiscard]] std::vector intersections(const polygon_t & polygon) const + { + std::vector result; + if (line_obstacle_tree_ptr) { + result = line_obstacle_tree_ptr->intersections(polygon); + } else { + boost::geometry::intersection(polygon, obstacles.lines, result); + } + if (point_obstacle_tree_ptr) { + const auto & point_result = point_obstacle_tree_ptr->intersections(polygon); + result.insert(result.end(), point_result.begin(), point_result.end()); + } else { + for (const auto & point : obstacles.points) + if (boost::geometry::intersects(polygon, point)) result.push_back(point); + } + return result; + } +}; + +/// @brief create a polygon from an object represented by a pose and a size +/// @param [in] pose pose of the object +/// @param [in] dimensions dimensions of the object +/// @param [in] buffer buffer to add to the dimensions of the object +/// @return polygon of the object +polygon_t createObjectPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions, + const double buffer); + +/// @brief create polygons from a set of predicted object +/// @param [in] objects objects from which to create polygons +/// @param [in] buffer buffer to add to the objects dimensions +/// @param [in] min_velocity objects with velocity lower will be ignored +/// @return polygons of the objects +multipolygon_t createObjectPolygons( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const double min_velocity); + +/// @brief add obstacles obtained from sensors to the given Obstacles object +/// @param[out] obstacles Obstacles object in which to add the sensor obstacles +/// @param[in] occupancy_grid occupancy grid +/// @param[in] pointcloud pointcloud +/// @param[in] masks masks used to discard some obstacles +/// @param[in] transform_listener object used to retrieve the latest transform +/// @param[in] target_frame frame of the returned obstacles +/// @param[in] obstacle_params obstacle parameters +void addSensorObstacles( + Obstacles & obstacles, const OccupancyGrid & occupancy_grid, const PointCloud & pointcloud, + const ObstacleMasks & masks, tier4_autoware_utils::TransformListener & transform_listener, + const std::string & target_frame, const ObstacleParameters & obstacle_params); + +/// @brief filter obstacles with the given negative and positive masks +/// @param[in] obstacles obstacles to filter +/// @param[in] masks masks used to discard some obstacles +/// @return obstacles that are inside the positive mask and outside of the negative masks +Obstacles filterObstacles(const Obstacles & obstacles, const ObstacleMasks & masks); +} // namespace obstacle_velocity_limiter +#endif // OBSTACLE_VELOCITY_LIMITER__OBSTACLES_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp new file mode 100644 index 0000000000000..9babbe8defb5a --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp @@ -0,0 +1,48 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__OCCUPANCY_GRID_UTILS_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__OCCUPANCY_GRID_UTILS_HPP_ + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include + +namespace obstacle_velocity_limiter +{ + +/// @brief mask gridmap cells that are inside the given polygons +/// @param[in, out] grid_map the grid map to modify +/// @param[in] polygons the polygons to mask from the grid map +void maskPolygons(grid_map::GridMap & grid_map, const ObstacleMasks & masks); + +/// @brief apply a threshold to the grid map +/// @param[in, out] grid_map the grid map to modify +/// @param[in] threshold cells above this value are set to the max value, the other are set to 0 +void threshold(grid_map::GridMap & grid_map, const float threshold); + +grid_map::GridMap convertToGridMap(const OccupancyGrid & occupancy_grid); + +/// @brief extract obstacles from an occupancy grid +/// @param[in] occupancy_grid input occupancy grid +/// @param[in] occupied_threshold threshold to use for identifying obstacles in the occupancy grid +/// @return extracted obstacle linestrings +multilinestring_t extractObstacles( + const grid_map::GridMap & grid_map, const OccupancyGrid & occupancy_grid); +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__OCCUPANCY_GRID_UTILS_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp new file mode 100644 index 0000000000000..cb8c812661bfc --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp @@ -0,0 +1,242 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__PARAMETERS_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__PARAMETERS_HPP_ + +#include +#include + +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ + +struct ObstacleParameters +{ + static constexpr auto DYN_SOURCE_PARAM = "obstacles.dynamic_source"; + static constexpr auto OCC_GRID_THRESH_PARAM = "obstacles.occupancy_grid_threshold"; + static constexpr auto BUFFER_PARAM = "obstacles.dynamic_obstacles_buffer"; + static constexpr auto MIN_VEL_PARAM = "obstacles.dynamic_obstacles_min_vel"; + static constexpr auto MAP_TAGS_PARAM = "obstacles.static_map_tags"; + static constexpr auto FILTERING_PARAM = "obstacles.filter_envelope"; + static constexpr auto IGNORE_ON_PATH_PARAM = "obstacles.ignore_obstacles_on_path"; + static constexpr auto IGNORE_DIST_PARAM = "obstacles.ignore_extra_distance"; + static constexpr auto RTREE_SEGMENTS_PARAM = "obstacles.rtree_min_segments"; + static constexpr auto RTREE_POINTS_PARAM = "obstacles.rtree_min_points"; + + enum { POINTCLOUD, OCCUPANCYGRID, STATIC_ONLY } dynamic_source = OCCUPANCYGRID; + int8_t occupancy_grid_threshold{}; + Float dynamic_obstacles_buffer{}; + Float dynamic_obstacles_min_vel{}; + std::vector static_map_tags{}; + bool filter_envelope; + bool ignore_on_path; + Float ignore_extra_distance; + size_t rtree_min_points{}; + size_t rtree_min_segments{}; + + ObstacleParameters() = default; + explicit ObstacleParameters(rclcpp::Node & node) + { + updateType(node, node.declare_parameter(DYN_SOURCE_PARAM)); + occupancy_grid_threshold = + static_cast(node.declare_parameter(OCC_GRID_THRESH_PARAM)); + dynamic_obstacles_buffer = static_cast(node.declare_parameter(BUFFER_PARAM)); + dynamic_obstacles_min_vel = static_cast(node.declare_parameter(MIN_VEL_PARAM)); + static_map_tags = node.declare_parameter>(MAP_TAGS_PARAM); + filter_envelope = node.declare_parameter(FILTERING_PARAM); + ignore_on_path = node.declare_parameter(IGNORE_ON_PATH_PARAM); + ignore_extra_distance = static_cast(node.declare_parameter(IGNORE_DIST_PARAM)); + updateRtreeMinPoints(node, static_cast(node.declare_parameter(RTREE_POINTS_PARAM))); + updateRtreeMinSegments( + node, static_cast(node.declare_parameter(RTREE_SEGMENTS_PARAM))); + } + + bool updateType(rclcpp::Node & node, const std::string & type) + { + if (type == "pointcloud") { + dynamic_source = POINTCLOUD; + } else if (type == "occupancy_grid") { + dynamic_source = OCCUPANCYGRID; + } else if (type == "static_only") { + dynamic_source = STATIC_ONLY; + } else { + dynamic_source = STATIC_ONLY; + RCLCPP_WARN( + node.get_logger(), "Unknown '%s' value: '%s'. Using default 'static_only'.", + DYN_SOURCE_PARAM, type.c_str()); + return false; + } + return true; + } + + bool updateRtreeMinPoints(rclcpp::Node & node, const int & size) + { + if (size < 0) { + RCLCPP_WARN( + node.get_logger(), "Min points for the Rtree must be positive. %d was given.", size); + return false; + } + rtree_min_segments = static_cast(size); + return true; + } + + bool updateRtreeMinSegments(rclcpp::Node & node, const int & size) + { + if (size < 0) { + RCLCPP_WARN( + node.get_logger(), "Min segments for the Rtree must be positive. %d was given.", size); + return false; + } + rtree_min_segments = static_cast(size); + return true; + } +}; + +struct ProjectionParameters +{ + static constexpr auto MODEL_PARAM = "simulation.model"; + static constexpr auto NBPOINTS_PARAM = "simulation.nb_points"; + static constexpr auto STEER_OFFSET_PARAM = "simulation.steering_offset"; + static constexpr auto DISTANCE_METHOD_PARAM = "simulation.distance_method"; + static constexpr auto DURATION_PARAM = "min_ttc"; + + enum { PARTICLE, BICYCLE } model = PARTICLE; + enum { EXACT, APPROXIMATION } distance_method = EXACT; + double duration{}; + double extra_length{}; + double velocity{}; + double heading{}; + // parameters specific to the bicycle model + int points_per_projection = 5; + double wheel_base{}; + double steering_angle{}; + double steering_angle_offset{}; + + ProjectionParameters() = default; + explicit ProjectionParameters(rclcpp::Node & node) + { + updateModel(node, node.declare_parameter(MODEL_PARAM)); + updateDistanceMethod(node, node.declare_parameter(DISTANCE_METHOD_PARAM)); + updateNbPoints(node, node.declare_parameter(NBPOINTS_PARAM)); + steering_angle_offset = node.declare_parameter(STEER_OFFSET_PARAM); + duration = node.declare_parameter(DURATION_PARAM); + } + + bool updateModel(rclcpp::Node & node, const std::string & model_str) + { + if (model_str == "particle") { + model = PARTICLE; + } else if (model_str == "bicycle") { + model = BICYCLE; + } else { + RCLCPP_WARN( + node.get_logger(), "Unknown projection model: '%s'. Using default PARTICLE model.", + model_str.c_str()); + return false; + } + return true; + } + + bool updateDistanceMethod(rclcpp::Node & node, const std::string & method_str) + { + if (method_str == "exact") { + distance_method = EXACT; + } else if (method_str == "approximation") { + distance_method = APPROXIMATION; + } else { + RCLCPP_WARN( + node.get_logger(), "Unknown distance calculation method: '%s'. Using default EXACT method.", + method_str.c_str()); + return false; + } + return true; + } + + bool updateNbPoints(rclcpp::Node & node, const int nb_points) + { + if (nb_points < 2) { + RCLCPP_WARN( + node.get_logger(), "Cannot use less than 2 points per projection. Using value %d instead.", + points_per_projection); + return false; + } + points_per_projection = nb_points; + return true; + } + + void update(const TrajectoryPoint & point) + { + velocity = point.longitudinal_velocity_mps; + heading = tf2::getYaw(point.pose.orientation); + steering_angle = point.front_wheel_angle_rad; + } +}; + +struct VelocityParameters +{ + static constexpr auto MIN_VEL_PARAM = "min_adjusted_velocity"; + static constexpr auto MAX_DECEL_PARAM = "max_deceleration"; + + Float min_velocity{}; + Float max_deceleration{}; + Float current_ego_velocity{}; + + VelocityParameters() = default; + explicit VelocityParameters(rclcpp::Node & node) + { + min_velocity = static_cast(node.declare_parameter(MIN_VEL_PARAM)); + max_deceleration = static_cast(node.declare_parameter(MAX_DECEL_PARAM)); + } +}; + +struct PreprocessingParameters +{ + static constexpr auto DOWNSAMPLING_PARAM = "trajectory_preprocessing.downsample_factor"; + static constexpr auto START_DIST_PARAM = "trajectory_preprocessing.start_distance"; + static constexpr auto CALC_STEER_PARAM = "trajectory_preprocessing.calculate_steering_angles"; + static constexpr auto MAX_LENGTH_PARAM = "trajectory_preprocessing.max_length"; + static constexpr auto MAX_DURATION_PARAM = "trajectory_preprocessing.max_duration"; + + int downsample_factor{}; + Float start_distance{}; + bool calculate_steering_angles{}; + Float max_length{}; + Float max_duration{}; + + PreprocessingParameters() = default; + explicit PreprocessingParameters(rclcpp::Node & node) + { + downsample_factor = node.declare_parameter(DOWNSAMPLING_PARAM); + start_distance = static_cast(node.declare_parameter(START_DIST_PARAM)); + calculate_steering_angles = node.declare_parameter(CALC_STEER_PARAM); + max_length = node.declare_parameter(MAX_LENGTH_PARAM); + max_duration = node.declare_parameter(MAX_DURATION_PARAM); + } + bool updateDownsampleFactor(const int new_downsample_factor) + { + if (new_downsample_factor > 0) { + downsample_factor = new_downsample_factor; + return true; + } + return false; + } +}; + +} // namespace obstacle_velocity_limiter +#endif // OBSTACLE_VELOCITY_LIMITER__PARAMETERS_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp new file mode 100644 index 0000000000000..9714a7003b1db --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp @@ -0,0 +1,54 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__POINTCLOUD_UTILS_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__POINTCLOUD_UTILS_HPP_ + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/types.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" + +#include + +#include +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ + +/// @brief return the pointcloud msg transformed and converted to PCL format +/// @param[in] pointcloud_msg pointcloud to transform +/// @param[in] transform_listener used to retrieve the latest transform +/// @param[in] target_frame frame of the returned pointcloud +/// @return PCL pointcloud +pcl::PointCloud::Ptr transformPointCloud( + const PointCloud & pointcloud_msg, tier4_autoware_utils::TransformListener & transform_listener, + const std::string & target_frame); + +/// @brief filter the pointcloud to keep only relevent points +/// @param[in,out] pointcloud to filter +/// @param[in] masks obstacle masks used to filter the pointcloud +void filterPointCloud(pcl::PointCloud::Ptr pointcloud, const ObstacleMasks & masks); + +/// @brief extract obstacles from the given pointcloud +/// @param[in] pointcloud input pointcloud +/// @return extracted obstacles +multipoint_t extractObstacles(const pcl::PointCloud::Ptr pointcloud); + +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__POINTCLOUD_UTILS_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp new file mode 100644 index 0000000000000..258ef8f01b5f5 --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp @@ -0,0 +1,66 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__TRAJECTORY_PREPROCESSING_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__TRAJECTORY_PREPROCESSING_HPP_ + +#include "obstacle_velocity_limiter/types.hpp" + +namespace obstacle_velocity_limiter +{ +/// @brief calculate trajectory index that is ahead of the given index by the given distance +/// @param[in] trajectory trajectory +/// @param[in] ego_idx index closest to the current ego position in the trajectory +/// @param[in] start_distance desired distance ahead of the ego_idx +/// @return trajectory index ahead of ego_idx by the start_distance +size_t calculateStartIndex( + const Trajectory & trajectory, const size_t ego_idx, const Float start_distance); + +/// @brief calculate trajectory index that is ahead of the given index by the given distance +/// @param[in] trajectory trajectory +/// @param[in] start_idx starting index +/// @param[in] max_length maximum length from start_idx to the returned index +/// @param[in] max_duration maximum duration from start_idx to the returned index +/// @return trajectory index ahead of the start_idx by at most the given length and duration +size_t calculateEndIndex( + const Trajectory & trajectory, const size_t start_idx, const Float max_length, + const Float max_duration); + +/// @brief downsample a trajectory, reducing its number of points by the given factor +/// @param[in] trajectory input trajectory +/// @param[in] start_idx starting index of the input trajectory +/// @param[in] end_idx ending index of the input trajectory +/// @param[in] factor factor used for downsampling +/// @return downsampled trajectory +Trajectory downsampleTrajectory( + const Trajectory & trajectory, const size_t start_idx, const size_t end_idx, const int factor); + +/// @brief recalculate the steering angle of the trajectory +/// @details uses the change in headings for calculation +/// @param[inout] trajectory input trajectory +/// @param[in] wheel_base wheel base of the vehicle +void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); + +/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory +/// @param[in] downsampled_trajectory downsampled trajectory +/// @param[in] trajectory input trajectory +/// @param[in] start_idx starting index of the downsampled trajectory relative to the input +/// @param[in] factor downsampling factor +/// @return input trajectory with the velocity profile of the downsampled trajectory +Trajectory copyDownsampledVelocity( + const Trajectory & downsampled_traj, Trajectory trajectory, const size_t start_idx, + const int factor); +} // namespace obstacle_velocity_limiter + +#endif // OBSTACLE_VELOCITY_LIMITER__TRAJECTORY_PREPROCESSING_HPP_ diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp new file mode 100644 index 0000000000000..f11b2ff87af23 --- /dev/null +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -0,0 +1,50 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_VELOCITY_LIMITER__TYPES_HPP_ +#define OBSTACLE_VELOCITY_LIMITER__TYPES_HPP_ + +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" + +#include +#include +#include +#include +#include + +namespace obstacle_velocity_limiter +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using nav_msgs::msg::OccupancyGrid; +using PointCloud = sensor_msgs::msg::PointCloud2; +using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); + +using point_t = tier4_autoware_utils::Point2d; +using multipoint_t = tier4_autoware_utils::MultiPoint2d; +using polygon_t = tier4_autoware_utils::Polygon2d; +using multipolygon_t = tier4_autoware_utils::MultiPolygon2d; +using segment_t = tier4_autoware_utils::Segment2d; +using linestring_t = tier4_autoware_utils::LineString2d; +using multilinestring_t = tier4_autoware_utils::MultiLineString2d; + +struct ObstacleMasks +{ + polygon_t positive_mask; // discard obstacles outside of this polygon + multipolygon_t negative_masks; // discard obstacles inside of these polygons +}; + +} // namespace obstacle_velocity_limiter +#endif // OBSTACLE_VELOCITY_LIMITER__TYPES_HPP_ diff --git a/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml b/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml new file mode 100644 index 0000000000000..4781ceda528bf --- /dev/null +++ b/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/obstacle_velocity_limiter/media/bicycle_distance.png b/planning/obstacle_velocity_limiter/media/bicycle_distance.png new file mode 100644 index 0000000000000..c2da2d1a4453e Binary files /dev/null and b/planning/obstacle_velocity_limiter/media/bicycle_distance.png differ diff --git a/planning/obstacle_velocity_limiter/media/bicycle_footprint.png b/planning/obstacle_velocity_limiter/media/bicycle_footprint.png new file mode 100644 index 0000000000000..4dbcde0759aaa Binary files /dev/null and b/planning/obstacle_velocity_limiter/media/bicycle_footprint.png differ diff --git a/planning/obstacle_velocity_limiter/media/ovl_off.png b/planning/obstacle_velocity_limiter/media/ovl_off.png new file mode 100644 index 0000000000000..ec4e6783b00c4 Binary files /dev/null and b/planning/obstacle_velocity_limiter/media/ovl_off.png differ diff --git a/planning/obstacle_velocity_limiter/media/ovl_on.png b/planning/obstacle_velocity_limiter/media/ovl_on.png new file mode 100644 index 0000000000000..9f5d5d6b168b0 Binary files /dev/null and b/planning/obstacle_velocity_limiter/media/ovl_on.png differ diff --git a/planning/obstacle_velocity_limiter/media/particle_distance.png b/planning/obstacle_velocity_limiter/media/particle_distance.png new file mode 100644 index 0000000000000..5e4319b0a21ec Binary files /dev/null and b/planning/obstacle_velocity_limiter/media/particle_distance.png differ diff --git a/planning/obstacle_velocity_limiter/media/particle_footprint.png b/planning/obstacle_velocity_limiter/media/particle_footprint.png new file mode 100644 index 0000000000000..da522b251b6d3 Binary files /dev/null and b/planning/obstacle_velocity_limiter/media/particle_footprint.png differ diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml new file mode 100644 index 0000000000000..ee2d3ecfbb914 --- /dev/null +++ b/planning/obstacle_velocity_limiter/package.xml @@ -0,0 +1,43 @@ + + + obstacle_velocity_limiter + 0.1.0 + Package to adjust velocities of a trajectory in order for the ride to feel safe + + Maxime CLEMENT + + Apache License 2.0 + + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + eigen + grid_map_msgs + grid_map_ros + grid_map_utils + lanelet2_core + lanelet2_extension + libboost-dev + motion_common + nav_msgs + pcl_ros + rclcpp + rclcpp_components + sensor_msgs + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py new file mode 100755 index 0000000000000..087325c6cc0a8 --- /dev/null +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from autoware_auto_planning_msgs.msg import Trajectory +import matplotlib.pyplot as plt +from nav_msgs.msg import Odometry +import numpy as np +import rclpy +from rclpy.node import Node + + +class TrajectoryVisualizer(Node): + def __init__(self): + + super().__init__("trajectory_visualizer") + + self.fig = plt.figure() + + self.sub_original_traj = self.create_subscription( + Trajectory, + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", + self.plotOriginalTrajectory, + 1, + ) + self.sub_adjusted_traj = self.create_subscription( + Trajectory, + "/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/trajectory", + self.plotAdjustedTrajectory, + 1, + ) + self.sub_odom = self.create_subscription( + Odometry, + "/localization/kinematic_state", + self.plotEgoVel, + 1, + ) + + self.initPlotTrajectoryVelocity() + + def plotOriginalTrajectory(self, original): + x = self.CalcArcLength(original) + y = self.ToVelList(original) + self.im1.set_data(x, y) + self.ax1.relim() + self.ax1.autoscale_view(True, True, True) + plt.draw() + plt.pause(0.01) + + def plotAdjustedTrajectory(self, adjusted): + x = self.CalcArcLength(adjusted) + y = self.ToVelList(adjusted) + self.im2.set_data(x, y) + self.ax1.relim() + self.ax1.autoscale_view(True, True, True) + plt.draw() + plt.pause(0.01) + + def plotEgoVel(self, odom_msg): + self.ego_vel.set_ydata( + [odom_msg.twist.twist.linear.x for _ in range(len(self.ego_vel.get_xdata()))] + ) + + def initPlotTrajectoryVelocity(self): + self.ax1 = plt.subplot(1, 1, 1) # row, col, index( 0: + s_arr.append(s_sum) + + for i in range(1, len(traj.points)): + p0 = traj.points[i - 1] + 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) + s_sum += ds + s_arr.append(s_sum) + return s_arr + + def ToVelList(self, traj): + v_list = [] + for p in traj.points: + v_list.append(p.longitudinal_velocity_mps) + return v_list + + +def main(args=None): + try: + rclpy.init(args=args) + node = TrajectoryVisualizer() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/planning/obstacle_velocity_limiter/src/debug.cpp b/planning/obstacle_velocity_limiter/src/debug.cpp new file mode 100644 index 0000000000000..fbbf48e69b023 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/debug.cpp @@ -0,0 +1,185 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/debug.hpp" + +#include "obstacle_velocity_limiter/parameters.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +namespace obstacle_velocity_limiter +{ +visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, const Float z) +{ + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.scale.x = 0.1; + marker.color.a = 1.0; + marker.header.frame_id = "map"; + for (const auto & point : ls) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + p.z = z; + marker.points.push_back(p); + } + return marker; +} + +visualization_msgs::msg::Marker makePolygonMarker(const polygon_t & polygon, const Float z) +{ + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.header.frame_id = "map"; + marker.scale.x = 0.1; + marker.color.a = 1.0; + geometry_msgs::msg::Point point; + point.z = z; + for (const auto & p : polygon.outer()) { + point.x = p.x(); + point.y = p.y(); + marker.points.push_back(point); + } + return marker; +} + +visualization_msgs::msg::MarkerArray makeDebugMarkers( + const Obstacles & obstacles, const std::vector & original_projections, + const std::vector & adjusted_projections, + const std::vector & original_footprints, + const std::vector & adjusted_footprints, const ObstacleMasks & obstacle_masks, + const Float marker_z) +{ + visualization_msgs::msg::MarkerArray debug_markers; + auto original_projections_id_offset = 0; + auto adjusted_projections_id_offset = 0; + for (auto i = 0ul; i < original_projections.size(); ++i) { + for (auto j = 0ul; j < original_projections[i].size(); ++j) { + auto marker = makeLinestringMarker(original_projections[i][j], marker_z); + marker.ns = "original_projections"; + marker.id = i + original_projections_id_offset + j; + marker.color.r = 0.7; + marker.color.b = 0.2; + debug_markers.markers.push_back(marker); + } + original_projections_id_offset += original_projections[i].size() - 1; + for (auto j = 0ul; j < adjusted_projections[i].size(); ++j) { + auto marker = makeLinestringMarker(adjusted_projections[i][j], marker_z); + marker.ns = "adjusted_projections"; + marker.id = i + adjusted_projections_id_offset + j; + marker.color.g = 0.7; + marker.color.b = 0.2; + debug_markers.markers.push_back(marker); + } + adjusted_projections_id_offset += adjusted_projections[i].size() - 1; + { + auto marker = makePolygonMarker(original_footprints[i], marker_z); + marker.ns = "original_footprints"; + marker.id = i; + marker.color.r = 0.7; + debug_markers.markers.push_back(marker); + } + { + auto marker = makePolygonMarker(adjusted_footprints[i], marker_z); + marker.ns = "adjusted_footprints"; + marker.id = i; + marker.color.g = 0.7; + debug_markers.markers.push_back(marker); + } + } + const auto max_id = original_projections.size() + + std::max(original_projections_id_offset, adjusted_projections_id_offset); + auto obs_id = 0lu; + for (const auto & ls : obstacles.lines) { + auto marker = makeLinestringMarker(ls, marker_z); + marker.ns = "obstacles"; + marker.id = obs_id++; + marker.color.b = 1.0; + debug_markers.markers.push_back(marker); + } + visualization_msgs::msg::Marker points_marker; + points_marker.type = visualization_msgs::msg::Marker::POINTS; + points_marker.header.frame_id = "map"; + points_marker.scale.x = 0.1; + points_marker.scale.y = 0.1; + points_marker.color.a = 1.0; + points_marker.color.b = 1.0; + points_marker.ns = "obstacles"; + points_marker.id = obs_id++; + geometry_msgs::msg::Point point; + point.z = marker_z; + for (const auto & p : obstacles.points) { + point.x = p.x(); + point.y = p.y(); + points_marker.points.push_back(point); + } + debug_markers.markers.push_back(points_marker); + + // Obstacle Masks + auto mask_id = 0lu; + for (const auto & negative_mask : obstacle_masks.negative_masks) { + auto marker = makePolygonMarker(negative_mask, marker_z); + marker.ns = "obstacle_masks"; + marker.id = mask_id++; + marker.color.r = 0.8; + debug_markers.markers.push_back(marker); + } + { + auto marker = makePolygonMarker(obstacle_masks.positive_mask, marker_z); + marker.ns = "obstacle_masks"; + marker.id = mask_id++; + marker.color.g = 0.8; + debug_markers.markers.push_back(marker); + } + + static auto prev_max_id = 0lu; + static auto prev_size = 0lu; + static auto prev_max_obs_id = 0lu; + static auto prev_max_mask_id = 0lu; + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETE; + marker.ns = "obstacles"; + for (auto delete_id = obs_id; delete_id < prev_max_obs_id; ++delete_id) { + marker.id = delete_id; + debug_markers.markers.push_back(marker); + } + marker.ns = "obstacle_masks"; + for (auto delete_id = mask_id; delete_id < prev_max_mask_id; ++delete_id) { + marker.id = delete_id; + debug_markers.markers.push_back(marker); + } + for (const auto & ns : {"original_projections", "adjusted_projections"}) { + marker.ns = ns; + for (auto delete_id = max_id; delete_id < prev_max_id; ++delete_id) { + marker.id = delete_id; + debug_markers.markers.push_back(marker); + } + } + for (const auto & ns : {"original_footprints", "adjusted_footprints"}) { + marker.ns = ns; + for (auto delete_id = original_projections.size(); delete_id < prev_size; ++delete_id) { + marker.id = delete_id; + debug_markers.markers.push_back(marker); + } + } + + prev_max_id = max_id; + prev_size = original_projections.size(); + prev_max_obs_id = obs_id; + prev_max_mask_id = mask_id; + return debug_markers; +} + +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/distance.cpp b/planning/obstacle_velocity_limiter/src/distance.cpp new file mode 100644 index 0000000000000..2e0ff52c1922c --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/distance.cpp @@ -0,0 +1,73 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/obstacles.hpp" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ +namespace bg = boost::geometry; + +std::optional distanceToClosestCollision( + const linestring_t & projection, const polygon_t & footprint, + const CollisionChecker & collision_checker, const ProjectionParameters & params) +{ + std::optional distance; + if (projection.empty()) return distance; + double min_dist = std::numeric_limits::max(); + for (const auto & obs_point : collision_checker.intersections(footprint)) { + if (params.distance_method == ProjectionParameters::EXACT) { + if (params.model == ProjectionParameters::PARTICLE) { + const auto euclidian_dist = bg::distance(obs_point, projection.front()); + const auto collision_heading = std::atan2( + obs_point.y() - projection.front().y(), obs_point.x() - projection.front().x()); + const auto angle = params.heading - collision_heading; + const auto long_dist = std::abs(std::cos(angle)) * euclidian_dist; + min_dist = std::min(min_dist, long_dist); + } else { // BICYCLE model with curved projection + min_dist = std::min(min_dist, arcDistance(projection.front(), params.heading, obs_point)); + } + } else { // APPROXIMATION + const auto euclidian_dist = bg::distance(obs_point, projection.front()); + min_dist = std::min(min_dist, euclidian_dist); + } + } + if (min_dist != std::numeric_limits::max()) distance = min_dist; + return distance; +} + +double arcDistance(const point_t & origin, const double heading, const point_t & target) +{ + // Circle passing through the origin and the target such that origin+heading is tangent + const auto d_normal = point_t{-std::sin(heading), std::cos(heading)}; + const auto midpoint = (origin + target) / 2; + const auto mid_to_target = target - midpoint; + const auto circle_center = + origin + (midpoint - origin).dot(mid_to_target) / (mid_to_target.dot(d_normal)) * d_normal; + const auto squared_radius = (circle_center - origin).squaredNorm(); + // Arc distance from angle between origin and target on the origin + const auto angle = + std::acos((2 * squared_radius - (origin - target).squaredNorm()) / (2 * squared_radius)); + return std::sqrt(squared_radius) * angle; +} +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/forward_projection.cpp b/planning/obstacle_velocity_limiter/src/forward_projection.cpp new file mode 100644 index 0000000000000..b4000389b19e8 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/forward_projection.cpp @@ -0,0 +1,142 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +namespace obstacle_velocity_limiter +{ + +segment_t forwardSimulatedSegment( + const geometry_msgs::msg::Point & origin, const ProjectionParameters & params) +{ + const auto length = params.velocity * params.duration + params.extra_length; + const auto from = point_t{origin.x, origin.y}; + const auto heading = params.heading; + const auto to = + point_t{from.x() + std::cos(heading) * length, from.y() + std::sin(heading) * length}; + return segment_t{from, to}; +} + +multilinestring_t bicycleProjectionLines( + const geometry_msgs::msg::Point & origin, const ProjectionParameters & params) +{ + multilinestring_t lines; + if (params.steering_angle_offset == 0.0) + lines.push_back(bicycleProjectionLine(origin, params, params.steering_angle)); + else + for (const auto offset : {params.steering_angle_offset, 0.0, -params.steering_angle_offset}) + lines.push_back(bicycleProjectionLine(origin, params, params.steering_angle + offset)); + return lines; +} + +linestring_t bicycleProjectionLine( + const geometry_msgs::msg::Point & origin, const ProjectionParameters & params, + const double steering_angle) +{ + linestring_t line; + line.reserve(params.points_per_projection); + line.emplace_back(origin.x, origin.y); + const auto dt = params.duration / (params.points_per_projection - 1); + const auto rotation_rate = params.velocity * std::tan(steering_angle) / params.wheel_base; + for (auto i = 1; i < params.points_per_projection; ++i) { + const auto t = i * dt; + const auto heading = params.heading + rotation_rate * t; + const auto length = params.velocity * t + params.extra_length; + line.emplace_back(origin.x + length * std::cos(heading), origin.y + length * std::sin(heading)); + } + return line; +} + +polygon_t generateFootprint(const multilinestring_t & lines, const double lateral_offset) +{ + polygon_t footprint; + if (lines.size() == 1) { + footprint = generateFootprint(lines.front(), lateral_offset); + } else if (lines.size() >= 3) { + // assumes 3 lines ordered from left to right + footprint = generateFootprint(lines[0], lines[1], lines[2], lateral_offset); + } + return footprint; +} + +polygon_t generateFootprint(const segment_t & segment, const double lateral_offset) +{ + return generateFootprint(linestring_t{segment.first, segment.second}, lateral_offset); +} + +polygon_t generateFootprint(const linestring_t & linestring, const double lateral_offset) +{ + namespace bg = boost::geometry; + multipolygon_t footprint; + namespace strategy = bg::strategy::buffer; + bg::buffer( + linestring, footprint, strategy::distance_symmetric(lateral_offset), + strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), + strategy::point_square()); + if (footprint.empty()) return {}; + return footprint[0]; +} + +polygon_t generateFootprint( + const linestring_t & left, const linestring_t & middle, const linestring_t & right, + const double lateral_offset) +{ + polygon_t footprint; + // calculate normal unit vector from the end of a line to generate its left/right points + constexpr auto perpendicular_point = + [](const point_t & a, const point_t & b, const double offset) { + const auto vector = (b - a).normalized(); + const auto normal_vector = point_t{-vector.y(), vector.x()}; + const auto point = b + (normal_vector * offset); + return point_t{point.x(), point.y()}; + }; + + footprint.outer().push_back(perpendicular_point(left[1], left[0], -lateral_offset)); + for (auto it = left.begin(); it != std::prev(left.end()); ++it) + footprint.outer().push_back(perpendicular_point(*it, *std::next(it), lateral_offset)); + // only use the left/right points at the end of the center line + { + footprint.outer().push_back( + perpendicular_point(middle[middle.size() - 2], middle.back(), lateral_offset)); + footprint.outer().push_back( + perpendicular_point(middle[middle.size() - 2], middle.back(), -lateral_offset)); + } + // points to the right of the right line are added in reverse + footprint.outer().push_back( + perpendicular_point(right[right.size() - 2], right.back(), -lateral_offset)); + for (auto it = right.rbegin(); it != std::prev(right.rend()); ++it) + footprint.outer().push_back(perpendicular_point(*it, *std::next(it), lateral_offset)); + footprint.outer().push_back(footprint.outer().front()); // close the polygon + return footprint; +} +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/map_utils.cpp b/planning/obstacle_velocity_limiter/src/map_utils.cpp new file mode 100644 index 0000000000000..5e39daef89c64 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/map_utils.cpp @@ -0,0 +1,53 @@ + +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/map_utils.hpp" + +#include "lanelet2_core/primitives/LineString.h" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include + +#include + +namespace obstacle_velocity_limiter +{ +multilinestring_t extractStaticObstacles( + const lanelet::LaneletMap & lanelet_map, const std::vector & tags) +{ + multilinestring_t lines; + linestring_t line; + linestring_t simplified_line; + for (const auto & ls : lanelet_map.lineStringLayer) { + if (isObstacle(ls, tags)) { + line.clear(); + simplified_line.clear(); + for (const auto & p : ls) line.push_back(point_t{p.x(), p.y()}); + boost::geometry::simplify(line, simplified_line, 0.5); + lines.push_back(simplified_line); + } + } + return lines; +} + +bool isObstacle(const lanelet::ConstLineString3d & ls, const std::vector & tags) +{ + constexpr auto no_type = ""; + const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); + return (type != no_type && std::find(tags.begin(), tags.end(), type) != tags.end()); +} +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp new file mode 100644 index 0000000000000..dbe7c87922564 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -0,0 +1,145 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/obstacle_velocity_limiter.hpp" + +#include "obstacle_velocity_limiter/distance.hpp" +#include "obstacle_velocity_limiter/forward_projection.hpp" + +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ + +Float calculateSafeVelocity( + const TrajectoryPoint & trajectory_point, const Float dist_to_collision, const Float time_buffer, + const Float min_adjusted_velocity) +{ + return std::min( + trajectory_point.longitudinal_velocity_mps, + std::max(min_adjusted_velocity, static_cast(dist_to_collision / time_buffer))); +} + +multipolygon_t createPolygonMasks( + const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, + const Float buffer, const Float min_vel) +{ + return createObjectPolygons(dynamic_obstacles, buffer, min_vel); +} + +std::vector createFootprintPolygons( + const std::vector & projected_linestrings, const Float lateral_offset) +{ + std::vector footprints; + footprints.reserve(projected_linestrings.size()); + for (const auto & linestrings : projected_linestrings) { + footprints.push_back(generateFootprint(linestrings, lateral_offset)); + } + return footprints; +} + +polygon_t createTrajectoryFootprint(const Trajectory & trajectory, const Float lateral_offset) +{ + linestring_t ls; + ls.reserve(trajectory.points.size()); + for (const auto & p : trajectory.points) ls.emplace_back(p.pose.position.x, p.pose.position.y); + return generateFootprint(ls, lateral_offset); +} + +polygon_t createEnvelopePolygon( + const Trajectory & trajectory, const size_t start_idx, ProjectionParameters & projection_params) +{ + polygon_t envelope_polygon; + const auto trajectory_size = trajectory.points.size() - start_idx; + if (trajectory_size < 2) return envelope_polygon; + + envelope_polygon.outer().resize(trajectory_size * 2 + 1); + for (size_t i = 0; i < trajectory_size; ++i) { + const auto & point = trajectory.points[i + start_idx]; + projection_params.update(point); + const auto forward_simulated_vector = + forwardSimulatedSegment(point.pose.position, projection_params); + envelope_polygon.outer()[i] = forward_simulated_vector.second; + const auto reverse_index = 2 * trajectory_size - i - 1; + envelope_polygon.outer()[reverse_index] = forward_simulated_vector.first; + } + envelope_polygon.outer().push_back(envelope_polygon.outer().front()); + boost::geometry::correct(envelope_polygon); + return envelope_polygon; +} + +polygon_t createEnvelopePolygon(const std::vector & footprints) +{ + multipolygon_t unions; + multipolygon_t result; + for (const auto & footprint : footprints) { + boost::geometry::union_(footprint, unions, result); + unions = result; + boost::geometry::clear(result); + } + if (unions.empty()) return {}; + return unions.front(); +} + +std::vector createProjectedLines( + const Trajectory & trajectory, ProjectionParameters & params) +{ + std::vector projections; + projections.reserve(trajectory.points.size()); + for (const auto & point : trajectory.points) { + params.update(point); + if (params.model == ProjectionParameters::PARTICLE) { + const auto projection = forwardSimulatedSegment(point.pose.position, params); + projections.push_back({{projection.first, projection.second}}); + } else { + projections.push_back(bicycleProjectionLines(point.pose.position, params)); + } + } + return projections; +} + +void limitVelocity( + Trajectory & trajectory, const CollisionChecker & collision_checker, + const std::vector & projections, const std::vector & footprints, + ProjectionParameters & projection_params, const VelocityParameters & velocity_params) +{ + Float time = 0.0; + for (size_t i = 0; i < trajectory.points.size(); ++i) { + auto & trajectory_point = trajectory.points[i]; + if (i > 0) { + const auto & prev_point = trajectory.points[i - 1]; + time += static_cast( + tier4_autoware_utils::calcDistance2d(prev_point, trajectory_point) / + prev_point.longitudinal_velocity_mps); + } + // First linestring is used to calculate distance + if (projections[i].empty()) continue; + projection_params.update(trajectory_point); + const auto dist_to_collision = distanceToClosestCollision( + projections[i][0], footprints[i], collision_checker, projection_params); + if (dist_to_collision) { + const auto min_feasible_velocity = + velocity_params.current_ego_velocity - velocity_params.max_deceleration * time; + trajectory_point.longitudinal_velocity_mps = std::max( + min_feasible_velocity, + calculateSafeVelocity( + trajectory_point, static_cast(*dist_to_collision - projection_params.extra_length), + static_cast(projection_params.duration), velocity_params.min_velocity)); + } + } +} +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp new file mode 100644 index 0000000000000..a623ee2a92742 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -0,0 +1,258 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp" + +#include "obstacle_velocity_limiter/debug.hpp" +#include "obstacle_velocity_limiter/forward_projection.hpp" +#include "obstacle_velocity_limiter/map_utils.hpp" +#include "obstacle_velocity_limiter/obstacle_velocity_limiter.hpp" +#include "obstacle_velocity_limiter/parameters.hpp" +#include "obstacle_velocity_limiter/trajectory_preprocessing.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace obstacle_velocity_limiter +{ +ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("obstacle_velocity_limiter", node_options), + preprocessing_params_(*this), + projection_params_(*this), + obstacle_params_(*this), + velocity_params_(*this) +{ + sub_trajectory_ = create_subscription( + "~/input/trajectory", 1, [this](const Trajectory::ConstSharedPtr msg) { onTrajectory(msg); }); + sub_occupancy_grid_ = create_subscription( + "~/input/occupancy_grid", 1, + [this](const OccupancyGrid::ConstSharedPtr msg) { occupancy_grid_ptr_ = msg; }); + sub_pointcloud_ = create_subscription( + "~/input/obstacle_pointcloud", rclcpp::QoS(1).best_effort(), + [this](const PointCloud::ConstSharedPtr msg) { pointcloud_ptr_ = msg; }); + sub_objects_ = create_subscription( + "~/input/dynamic_obstacles", 1, + [this](const PredictedObjects::ConstSharedPtr msg) { dynamic_obstacles_ptr_ = msg; }); + sub_odom_ = create_subscription( + "~/input/odometry", rclcpp::QoS{1}, [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { + current_ego_velocity_ = static_cast(msg->twist.twist.linear.x); + }); + map_sub_ = create_subscription( + "~/input/map", rclcpp::QoS{1}.transient_local(), + [this](const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); + static_map_obstacles_ = + extractStaticObstacles(*lanelet_map_ptr_, obstacle_params_.static_map_tags); + }); + + pub_trajectory_ = create_publisher("~/output/trajectory", 1); + pub_debug_markers_ = + create_publisher("~/output/debug_markers", 1); + pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); + + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); + vehicle_front_offset_ = static_cast(vehicle_info.max_longitudinal_offset_m); + + projection_params_.wheel_base = vehicle_info.wheel_base_m; + projection_params_.extra_length = vehicle_front_offset_ + distance_buffer_; + + set_param_res_ = + add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); +} + +rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParameter( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if (parameter.get_name() == "distance_buffer") { + distance_buffer_ = static_cast(parameter.as_double()); + projection_params_.extra_length = vehicle_front_offset_ + distance_buffer_; + // Preprocessing parameters + } else if (parameter.get_name() == PreprocessingParameters::START_DIST_PARAM) { + preprocessing_params_.start_distance = static_cast(parameter.as_double()); + } else if (parameter.get_name() == PreprocessingParameters::DOWNSAMPLING_PARAM) { + if (!preprocessing_params_.updateDownsampleFactor(parameter.as_int())) { + result.successful = false; + result.reason = "downsample_factor must be positive"; + } + } else if (parameter.get_name() == PreprocessingParameters::CALC_STEER_PARAM) { + preprocessing_params_.calculate_steering_angles = parameter.as_bool(); + } else if (parameter.get_name() == PreprocessingParameters::MAX_LENGTH_PARAM) { + preprocessing_params_.max_length = static_cast(parameter.as_double()); + } else if (parameter.get_name() == PreprocessingParameters::MAX_DURATION_PARAM) { + preprocessing_params_.max_duration = static_cast(parameter.as_double()); + // Velocity parameters + } else if (parameter.get_name() == VelocityParameters::MIN_VEL_PARAM) { + velocity_params_.min_velocity = static_cast(parameter.as_double()); + } else if (parameter.get_name() == VelocityParameters::MAX_DECEL_PARAM) { + velocity_params_.max_deceleration = static_cast(parameter.as_double()); + // Obstacle parameters + } else if (parameter.get_name() == ProjectionParameters::DURATION_PARAM) { + const auto min_ttc = static_cast(parameter.as_double()); + if (min_ttc > 0.0) { + projection_params_.duration = min_ttc; + } else { + result.successful = false; + result.reason = "duration of forward projection must be positive"; + } + } else if (parameter.get_name() == ObstacleParameters::DYN_SOURCE_PARAM) { + if (!obstacle_params_.updateType(*this, parameter.as_string())) { + result.successful = false; + result.reason = "dynamic_source value must be 'pointcloud' or 'occupancy_grid'"; + } + } else if (parameter.get_name() == ObstacleParameters::OCC_GRID_THRESH_PARAM) { + obstacle_params_.occupancy_grid_threshold = static_cast(parameter.as_int()); + } else if (parameter.get_name() == ObstacleParameters::BUFFER_PARAM) { + obstacle_params_.dynamic_obstacles_buffer = static_cast(parameter.as_double()); + } else if (parameter.get_name() == ObstacleParameters::MIN_VEL_PARAM) { + obstacle_params_.dynamic_obstacles_min_vel = static_cast(parameter.as_double()); + } else if (parameter.get_name() == ObstacleParameters::MAP_TAGS_PARAM) { + obstacle_params_.static_map_tags = parameter.as_string_array(); + if (lanelet_map_ptr_) + static_map_obstacles_ = + extractStaticObstacles(*lanelet_map_ptr_, obstacle_params_.static_map_tags); + } else if (parameter.get_name() == ObstacleParameters::FILTERING_PARAM) { + obstacle_params_.filter_envelope = parameter.as_bool(); + } else if (parameter.get_name() == ObstacleParameters::IGNORE_ON_PATH_PARAM) { + obstacle_params_.ignore_on_path = parameter.as_bool(); + } else if (parameter.get_name() == ObstacleParameters::IGNORE_DIST_PARAM) { + obstacle_params_.ignore_extra_distance = static_cast(parameter.as_double()); + } else if (parameter.get_name() == ObstacleParameters::RTREE_POINTS_PARAM) { + obstacle_params_.updateRtreeMinPoints(*this, static_cast(parameter.as_int())); + } else if (parameter.get_name() == ObstacleParameters::RTREE_SEGMENTS_PARAM) { + obstacle_params_.updateRtreeMinSegments(*this, static_cast(parameter.as_int())); + // Projection parameters + } else if (parameter.get_name() == ProjectionParameters::MODEL_PARAM) { + if (!projection_params_.updateModel(*this, parameter.as_string())) { + result.successful = false; + result.reason = "Unknown forward projection model"; + } + } else if (parameter.get_name() == ProjectionParameters::NBPOINTS_PARAM) { + if (!projection_params_.updateNbPoints(*this, static_cast(parameter.as_int()))) { + result.successful = false; + result.reason = "number of points for projections must be at least 2"; + } + } else if (parameter.get_name() == ProjectionParameters::STEER_OFFSET_PARAM) { + projection_params_.steering_angle_offset = parameter.as_double(); + } else if (parameter.get_name() == ProjectionParameters::DISTANCE_METHOD_PARAM) { + projection_params_.updateDistanceMethod(*this, parameter.as_string()); + } else { + RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); + result.successful = false; + } + } + return result; +} + +void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr msg) +{ + const auto t_start = std::chrono::system_clock::now(); + const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); + if (!current_pose_ptr) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), rcutils_duration_value_t(1000), "Waiting for current pose"); + return; + } + const auto ego_idx = + autoware::motion::motion_common::findNearestIndex(msg->points, current_pose_ptr->pose); + if (!validInputs(ego_idx)) return; + auto original_traj = *msg; + if (preprocessing_params_.calculate_steering_angles) + calculateSteeringAngles(original_traj, projection_params_.wheel_base); + velocity_params_.current_ego_velocity = *current_ego_velocity_; + const auto start_idx = + calculateStartIndex(original_traj, *ego_idx, preprocessing_params_.start_distance); + const auto end_idx = calculateEndIndex( + original_traj, start_idx, preprocessing_params_.max_length, preprocessing_params_.max_duration); + Trajectory downsampled_traj = downsampleTrajectory( + original_traj, start_idx, end_idx, preprocessing_params_.downsample_factor); + ObstacleMasks obstacle_masks; + obstacle_masks.negative_masks = createPolygonMasks( + *dynamic_obstacles_ptr_, obstacle_params_.dynamic_obstacles_buffer, + obstacle_params_.dynamic_obstacles_min_vel); + if (obstacle_params_.ignore_on_path) + obstacle_masks.negative_masks.push_back(createTrajectoryFootprint( + *msg, vehicle_lateral_offset_ + obstacle_params_.ignore_extra_distance)); + const auto projected_linestrings = createProjectedLines(downsampled_traj, projection_params_); + const auto footprint_polygons = + createFootprintPolygons(projected_linestrings, vehicle_lateral_offset_); + Obstacles obstacles; + obstacles.lines = static_map_obstacles_; + if (obstacle_params_.dynamic_source != ObstacleParameters::STATIC_ONLY) { + if (obstacle_params_.filter_envelope) + obstacle_masks.positive_mask = createEnvelopePolygon(footprint_polygons); + addSensorObstacles( + obstacles, *occupancy_grid_ptr_, *pointcloud_ptr_, obstacle_masks, transform_listener_, + original_traj.header.frame_id, obstacle_params_); + } + limitVelocity( + downsampled_traj, + CollisionChecker( + obstacles, obstacle_params_.rtree_min_points, obstacle_params_.rtree_min_segments), + projected_linestrings, footprint_polygons, projection_params_, velocity_params_); + auto safe_trajectory = copyDownsampledVelocity( + downsampled_traj, original_traj, start_idx, preprocessing_params_.downsample_factor); + safe_trajectory.header.stamp = now(); + + pub_trajectory_->publish(safe_trajectory); + + const auto t_end = std::chrono::system_clock::now(); + const auto runtime = std::chrono::duration_cast(t_end - t_start); + pub_runtime_->publish(std_msgs::msg::Int64().set__data(runtime.count())); + + if (pub_debug_markers_->get_subscription_count() > 0) { + const auto safe_projected_linestrings = + createProjectedLines(downsampled_traj, projection_params_); + const auto safe_footprint_polygons = + createFootprintPolygons(safe_projected_linestrings, vehicle_lateral_offset_); + pub_debug_markers_->publish(makeDebugMarkers( + obstacles, projected_linestrings, safe_projected_linestrings, footprint_polygons, + safe_footprint_polygons, obstacle_masks, occupancy_grid_ptr_->info.origin.position.z)); + } +} + +bool ObstacleVelocityLimiterNode::validInputs( + const std::experimental::fundamentals_v1::optional & ego_idx) +{ + constexpr auto one_sec = rcutils_duration_value_t(1000); + if (!occupancy_grid_ptr_) + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), one_sec, "Occupancy grid not yet received"); + if (!dynamic_obstacles_ptr_) + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), one_sec, "Dynamic obstacles not yet received"); + if (!ego_idx) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Cannot calculate ego index on the trajectory"); + if (!current_ego_velocity_) + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), one_sec, "Current ego velocity not yet received"); + + return occupancy_grid_ptr_ && dynamic_obstacles_ptr_ && ego_idx && current_ego_velocity_; +} +} // namespace obstacle_velocity_limiter + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(obstacle_velocity_limiter::ObstacleVelocityLimiterNode) diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp new file mode 100644 index 0000000000000..ac8444a508be8 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -0,0 +1,94 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/obstacles.hpp" + +#include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" +#include "obstacle_velocity_limiter/pointcloud_utils.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +namespace obstacle_velocity_limiter +{ +polygon_t createObjectPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions, + const double buffer) +{ + // rename + const auto x = pose.position.x; + const auto y = pose.position.y; + const auto h = dimensions.x + buffer; + const auto w = dimensions.y + buffer; + const auto yaw = tf2::getYaw(pose.orientation); + + // create base polygon + polygon_t obj_poly; + boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( + -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); + + // rotate polygon(yaw) + boost::geometry::strategy::transform::rotate_transformer + rotate(-yaw); // anti-clockwise -> :clockwise rotation + polygon_t rotate_obj_poly; + boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); + + // translate polygon(x, y) + boost::geometry::strategy::transform::translate_transformer translate(x, y); + polygon_t translate_obj_poly; + boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +multipolygon_t createObjectPolygons( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const double min_velocity) +{ + multipolygon_t polygons; + for (const auto & object : objects.objects) { + if ( + object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_velocity || + object.kinematics.initial_twist_with_covariance.twist.linear.x <= -min_velocity) { + polygons.push_back(createObjectPolygon( + object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + } + } + return polygons; +} + +void addSensorObstacles( + Obstacles & obstacles, const OccupancyGrid & occupancy_grid, const PointCloud & pointcloud, + const ObstacleMasks & masks, tier4_autoware_utils::TransformListener & transform_listener, + const std::string & target_frame, const ObstacleParameters & obstacle_params) +{ + if (obstacle_params.dynamic_source == ObstacleParameters::OCCUPANCYGRID) { + auto grid_map = convertToGridMap(occupancy_grid); + threshold(grid_map, obstacle_params.occupancy_grid_threshold); + maskPolygons(grid_map, masks); + const auto obstacle_lines = extractObstacles(grid_map, occupancy_grid); + obstacles.lines.insert(obstacles.lines.end(), obstacle_lines.begin(), obstacle_lines.end()); + } else if (obstacle_params.dynamic_source == ObstacleParameters::POINTCLOUD) { + auto pcd = transformPointCloud(pointcloud, transform_listener, target_frame); + filterPointCloud(pcd, masks); + obstacles.points = extractObstacles(pcd); + } +} +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp new file mode 100644 index 0000000000000..72b9bf6431f03 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp @@ -0,0 +1,94 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" + +#include "obstacle_velocity_limiter/types.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace obstacle_velocity_limiter +{ +void maskPolygons(grid_map::GridMap & grid_map, const ObstacleMasks & obstacle_masks) +{ + constexpr auto convert = [](const polygon_t & in) { + grid_map::Polygon out; + for (const auto & p : in.outer()) out.addVertex(p); + return out; + }; + + auto & layer = grid_map["layer"]; + + if (!obstacle_masks.positive_mask.outer().empty()) { + const auto layer_copy = grid_map["layer"]; + layer.setConstant(0.0); + grid_map::Position position; + for (grid_map_utils::PolygonIterator iterator(grid_map, convert(obstacle_masks.positive_mask)); + !iterator.isPastEnd(); ++iterator) + layer((*iterator)(0), (*iterator)(1)) = layer_copy((*iterator)(0), (*iterator)(1)); + } + + for (const auto & negative_mask : obstacle_masks.negative_masks) + for (grid_map_utils::PolygonIterator iterator(grid_map, convert(negative_mask)); + !iterator.isPastEnd(); ++iterator) + layer((*iterator)(0), (*iterator)(1)) = 0.0; +} + +void threshold(grid_map::GridMap & grid_map, const float threshold) +{ + for (grid_map::GridMapIterator iter(grid_map); !iter.isPastEnd(); ++iter) { + auto & val = grid_map.at("layer", *iter); + if (val < threshold) + val = 0.0; + else + val = 127; + } +} + +grid_map::GridMap convertToGridMap(const OccupancyGrid & occupancy_grid) +{ + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + return grid_map; +} + +multilinestring_t extractObstacles( + const grid_map::GridMap & grid_map, const OccupancyGrid & occupancy_grid) +{ + cv::Mat cv_image; + grid_map::GridMapCvConverter::toImage(grid_map, "layer", CV_8UC1, cv_image); + cv::dilate(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), 2); + cv::erode(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), 2); + std::vector> contours; + cv::findContours(cv_image, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE); + multilinestring_t obstacles; + const auto & info = occupancy_grid.info; + for (const auto & contour : contours) { + linestring_t line; + for (const auto & point : contour) { + line.emplace_back( + (info.width - 1.0 - point.y) * info.resolution + info.origin.position.x, + (info.height - 1.0 - point.x) * info.resolution + info.origin.position.y); + } + obstacles.emplace_back(line); + } + return obstacles; +} +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp new file mode 100644 index 0000000000000..4f9ca6c5bc3e7 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp @@ -0,0 +1,105 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/pointcloud_utils.hpp" + +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace obstacle_velocity_limiter +{ + +pcl::PointCloud::Ptr transformPointCloud( + const PointCloud & pointcloud_msg, tier4_autoware_utils::TransformListener & transform_listener, + const std::string & target_frame) +{ + const auto & header = pointcloud_msg.header; + const auto transform = transform_listener.getTransform( + target_frame, header.frame_id, header.stamp, rclcpp::Duration::from_nanoseconds(0)); + const Eigen::Matrix4f transform_matrix = + tf2::transformToEigen(transform->transform).matrix().cast(); + + PointCloud transformed_msg; + pcl_ros::transformPointCloud(transform_matrix, pointcloud_msg, transformed_msg); + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(transformed_msg, transformed_pointcloud); + return pcl::PointCloud::Ptr( + new pcl::PointCloud(std::move(transformed_pointcloud))); +} + +void filterPointCloud(pcl::PointCloud::Ptr pointcloud, const ObstacleMasks & masks) +{ + pcl::PointCloud::Ptr polygon_cloud_ptr(new pcl::PointCloud); + if (!masks.positive_mask.outer().empty()) { + for (const auto & p : masks.positive_mask.outer()) + polygon_cloud_ptr->push_back(pcl::PointXYZ(p.x(), p.y(), 0)); + std::vector polygons_idx(1); + polygons_idx.front().vertices.resize(masks.positive_mask.outer().size()); + std::iota(polygons_idx.front().vertices.begin(), polygons_idx.front().vertices.end(), 0); + pcl::CropHull crop; + crop.setInputCloud(pointcloud); + crop.setDim(2); + crop.setHullCloud(polygon_cloud_ptr); + crop.setHullIndices(polygons_idx); + crop.setCropOutside(true); + crop.filter(*pointcloud); + } + + pcl::PointCloud::Ptr mask_cloud_ptr(new pcl::PointCloud); + std::vector masks_idx(masks.negative_masks.size()); + size_t start_idx = 0; + for (size_t i = 0; i < masks.negative_masks.size(); ++i) { + const auto & polygon_mask = masks.negative_masks[i]; + const auto mask_size = polygon_mask.outer().size(); + for (const auto & p : polygon_mask.outer()) + mask_cloud_ptr->push_back(pcl::PointXYZ(p.x(), p.y(), 0)); + auto & mask_idx = masks_idx[i]; + mask_idx.vertices.resize(mask_size); + std::iota(mask_idx.vertices.begin(), mask_idx.vertices.end(), start_idx); + start_idx += mask_size; + } + pcl::CropHull crop_masks; + crop_masks.setInputCloud(pointcloud); + crop_masks.setDim(2); + crop_masks.setHullCloud(mask_cloud_ptr); + crop_masks.setHullIndices(masks_idx); + crop_masks.setCropOutside(false); + crop_masks.filter(*pointcloud); +} + +multipoint_t extractObstacles(const pcl::PointCloud::Ptr pointcloud_ptr) +{ + multipoint_t obstacles; + if (pointcloud_ptr->empty()) return obstacles; + obstacles.reserve(pointcloud_ptr->size()); + + for (const auto & point : *pointcloud_ptr) { + obstacles.push_back({point_t{point.x, point.y}}); + } + return obstacles; +} + +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/trajectory_preprocessing.cpp b/planning/obstacle_velocity_limiter/src/trajectory_preprocessing.cpp new file mode 100644 index 0000000000000..6296d9cc5af69 --- /dev/null +++ b/planning/obstacle_velocity_limiter/src/trajectory_preprocessing.cpp @@ -0,0 +1,102 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/trajectory_preprocessing.hpp" + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace obstacle_velocity_limiter +{ + +size_t calculateStartIndex( + const Trajectory & trajectory, const size_t ego_idx, const Float start_distance) +{ + auto dist = 0.0; + auto idx = ego_idx; + while (idx + 1 < trajectory.points.size() && dist < start_distance) { + dist += + tier4_autoware_utils::calcDistance2d(trajectory.points[idx], trajectory.points[idx + 1]); + ++idx; + } + return idx; +} + +size_t calculateEndIndex( + const Trajectory & trajectory, const size_t start_idx, const Float max_length, + const Float max_duration) +{ + auto length = 0.0; + auto duration = 0.0; + auto idx = start_idx; + while (idx + 1 < trajectory.points.size() && length < max_length && duration < max_duration) { + const auto length_d = + tier4_autoware_utils::calcDistance2d(trajectory.points[idx], trajectory.points[idx + 1]); + length += length_d; + if (trajectory.points[idx].longitudinal_velocity_mps > 0.0) + duration += length_d / trajectory.points[idx].longitudinal_velocity_mps; + ++idx; + } + return idx; +} + +Trajectory downsampleTrajectory( + const Trajectory & trajectory, const size_t start_idx, const size_t end_idx, const int factor) +{ + if (factor < 1) return trajectory; + Trajectory downsampled_traj; + downsampled_traj.header = trajectory.header; + downsampled_traj.points.reserve((end_idx - start_idx) / factor); + for (size_t i = start_idx; i <= end_idx; i += factor) + downsampled_traj.points.push_back(trajectory.points[i]); + return downsampled_traj; +} + +void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base) +{ + auto t = 0.0; + auto prev_point = trajectory.points.front(); + auto prev_heading = tf2::getYaw(prev_point.pose.orientation); + for (auto i = 1ul; i < trajectory.points.size(); ++i) { + const auto & prev_point = trajectory.points[i - 1]; + auto & point = trajectory.points[i]; + const auto dt = tier4_autoware_utils::calcDistance2d(prev_point, point) / + prev_point.longitudinal_velocity_mps; + t += dt; + const auto heading = tf2::getYaw(point.pose.orientation); + const auto d_heading = heading - prev_heading; + prev_heading = heading; + point.front_wheel_angle_rad = + std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt); + } +} + +Trajectory copyDownsampledVelocity( + const Trajectory & downsampled_traj, Trajectory trajectory, const size_t start_idx, + const int factor) +{ + const auto size = std::min(downsampled_traj.points.size(), trajectory.points.size()); + for (size_t i = 0; i < size; ++i) { + trajectory.points[start_idx + i * factor].longitudinal_velocity_mps = + downsampled_traj.points[i].longitudinal_velocity_mps; + } + return trajectory; +} +} // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp new file mode 100644 index 0000000000000..e9cf8d9ad4dd6 --- /dev/null +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -0,0 +1,233 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/distance.hpp" +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/types.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include + +#include + +#include +#include + +const auto point_in_polygon = [](const auto x, const auto y, const auto & polygon) { + return std::find_if(polygon.outer().begin(), polygon.outer().end(), [=](const auto & pt) { + return pt.x() == x && pt.y() == y; + }) != polygon.outer().end(); +}; + +TEST(TestCollisionDistance, distanceToClosestCollision) +{ + using obstacle_velocity_limiter::CollisionChecker; + using obstacle_velocity_limiter::distanceToClosestCollision; + using obstacle_velocity_limiter::linestring_t; + using obstacle_velocity_limiter::polygon_t; + + obstacle_velocity_limiter::ProjectionParameters params; + params.model = obstacle_velocity_limiter::ProjectionParameters::PARTICLE; + params.heading = 0.0; + linestring_t vector = {{0.0, 0.0}, {5.0, 0.0}}; + polygon_t footprint; + footprint.outer() = {{0.0, 1.0}, {5.0, 1.0}, {5.0, -1.0}, {0.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + obstacle_velocity_limiter::Obstacles obstacles; + + std::optional result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(-1.0, 0.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(1.0, 2.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(4.0, 0.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, 4.0); + + obstacles.points.emplace_back(3.0, 0.5); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, 3.0); + + obstacles.points.emplace_back(2.5, -0.75); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, 2.5); + + // Change vector and footprint + vector = linestring_t{{0.0, 0.0}, {5.0, 5.0}}; + params.heading = M_PI_4; + footprint.outer() = {{-1.0, 1.0}, {4.0, 6.0}, {6.0, 4.0}, {1.0, -1.0}}; + boost::geometry::correct(footprint); // avoid bugs with malformed polygon + obstacles.points.clear(); + obstacles.lines.clear(); + + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_FALSE(result.has_value()); + + obstacles.points.emplace_back(4.0, 4.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, std::sqrt(2 * 4.0 * 4.0)); + + obstacles.points.emplace_back(1.0, 2.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.121, 1e-3); + + obstacles.lines.push_back(linestring_t{{-2.0, 2.0}, {3.0, -1.0}}); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.354, 1e-3); + + obstacles.lines.push_back(linestring_t{{-1.5, 1.5}, {0.0, 0.5}}); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.141, 1e-3); + + obstacles.lines.push_back(linestring_t{{0.5, 1.0}, {0.5, -0.5}}); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.0, 1e-3); + + obstacles.points.clear(); + obstacles.lines.clear(); + obstacles.lines.push_back(linestring_t{{0.5, 1.0}, {0.5, 0.0}, {1.5, 0.0}}); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 0.353, 1e-3); + + // Change vector (opposite direction) + params.heading = -3 * M_PI_4; + vector = linestring_t{{5.0, 5.0}, {0.0, 0.0}}; + obstacles.points.clear(); + obstacles.lines.clear(); + + obstacles.points.emplace_back(1.0, 1.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_DOUBLE_EQ(*result, std::sqrt(2 * 4.0 * 4.0)); + + obstacles.points.emplace_back(4.0, 3.0); + result = + distanceToClosestCollision(vector, footprint, CollisionChecker(obstacles, 0lu, 0lu), params); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.121, 1e-3); +} + +TEST(TestCollisionDistance, arcDistance) +{ + using obstacle_velocity_limiter::arcDistance; + using obstacle_velocity_limiter::point_t; + + auto EPS = 1e-2; + + EXPECT_NEAR(arcDistance({0, 0}, M_PI_2, {1, 1}), M_PI_2, EPS); + EXPECT_NEAR(arcDistance({0, 0}, M_PI_2, {-1, -1}), M_PI_2, EPS); + EXPECT_NEAR(arcDistance({0, 0}, 0, {1, 1}), M_PI_2, EPS); + EXPECT_NEAR(arcDistance({0, 0}, 0, {0, 1}), M_PI_2, EPS); + EXPECT_NEAR(arcDistance({0, 0}, 0, {0, -1}), M_PI_2, EPS); + EXPECT_NEAR(arcDistance({0, 0}, 0, {1, 0.5}), 1.15, EPS); + EXPECT_NEAR(arcDistance({0, 0}, 0, {0.1, 0.5}), 0.71, EPS); + EXPECT_NEAR(arcDistance({0, 0.2}, 0.463646716, {0.4, 0.2}), 0.41, EPS); + EXPECT_NEAR(arcDistance({0, 0.0}, -M_PI_4, {1.0, 0.0}), 1.11, EPS); + EXPECT_NEAR(arcDistance({1, 2.0}, -M_PI_2, {0.0, 1.0}), M_PI_2, EPS); + EXPECT_NEAR(arcDistance({-0.6, -0.4}, -M_PI_4, {0.4, 0.2}), 1.59, EPS); + // Edge cases: target "behind" the origin leads to a reverse distance + EXPECT_NEAR(arcDistance({0, 0}, 0, {-1, -1}), M_PI_2, EPS); +} + +TEST(TestCollisionDistance, createObjPolygons) +{ + using autoware_auto_perception_msgs::msg::PredictedObject; + using autoware_auto_perception_msgs::msg::PredictedObjects; + using obstacle_velocity_limiter::createObjectPolygons; + + PredictedObjects objects; + + auto polygons = createObjectPolygons(objects, 0.0, 0.0); + EXPECT_TRUE(polygons.empty()); + + PredictedObject object1; + object1.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; + object1.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; + object1.kinematics.initial_pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(0.0); + object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; + object1.shape.dimensions.x = 1.0; + object1.shape.dimensions.y = 1.0; + objects.objects.push_back(object1); + + polygons = createObjectPolygons(objects, 0.0, 1.0); + EXPECT_TRUE(polygons.empty()); + + polygons = createObjectPolygons(objects, 0.0, 0.0); + ASSERT_EQ(polygons.size(), 1ul); + EXPECT_TRUE(point_in_polygon(0.5, 0.5, polygons[0])); + EXPECT_TRUE(point_in_polygon(0.5, -0.5, polygons[0])); + EXPECT_TRUE(point_in_polygon(-0.5, 0.5, polygons[0])); + EXPECT_TRUE(point_in_polygon(-0.5, -0.5, polygons[0])); + + polygons = createObjectPolygons(objects, 1.0, 0.0); + ASSERT_EQ(polygons.size(), 1ul); + EXPECT_TRUE(point_in_polygon(1.0, 1.0, polygons[0])); + EXPECT_TRUE(point_in_polygon(1.0, -1.0, polygons[0])); + EXPECT_TRUE(point_in_polygon(-1.0, 1.0, polygons[0])); + EXPECT_TRUE(point_in_polygon(-1.0, -1.0, polygons[0])); + + PredictedObject object2; + object2.kinematics.initial_pose_with_covariance.pose.position.x = 10.0; + object2.kinematics.initial_pose_with_covariance.pose.position.y = 10.0; + object2.kinematics.initial_pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(M_PI_2); + object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; + object2.shape.dimensions.x = 2.0; + object2.shape.dimensions.y = 1.0; + objects.objects.push_back(object2); + + polygons = createObjectPolygons(objects, 0.0, 2.0); + ASSERT_EQ(polygons.size(), 1ul); + EXPECT_TRUE(point_in_polygon(10.5, 11.0, polygons[0])); + EXPECT_TRUE(point_in_polygon(10.5, 9.0, polygons[0])); + EXPECT_TRUE(point_in_polygon(9.5, 11.0, polygons[0])); + EXPECT_TRUE(point_in_polygon(9.5, 9.0, polygons[0])); + + polygons = createObjectPolygons(objects, 0.0, 0.0); + EXPECT_EQ(polygons.size(), 2ul); +} diff --git a/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp new file mode 100644 index 0000000000000..3275bf1ab5506 --- /dev/null +++ b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp @@ -0,0 +1,165 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/forward_projection.hpp" +#include "obstacle_velocity_limiter/types.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +#include +#include + +#include + +constexpr auto EPS = 1e-15; +constexpr auto EPS_APPROX = 1e-3; + +TEST(TestForwardProjection, forwardSimulatedSegment) +{ + using obstacle_velocity_limiter::forwardSimulatedSegment; + using obstacle_velocity_limiter::ProjectionParameters; + using obstacle_velocity_limiter::segment_t; + + geometry_msgs::msg::Point point; + point.x = 0.0; + point.y = 0.0; + ProjectionParameters params; + params.model = ProjectionParameters::PARTICLE; + + const auto check_vector = [&](const auto expected_vector_length) { + params.heading = 0.0; + auto vector = forwardSimulatedSegment(point, params); + EXPECT_DOUBLE_EQ(vector.first.x(), point.x); + EXPECT_DOUBLE_EQ(vector.first.y(), point.y); + EXPECT_DOUBLE_EQ(vector.second.x(), expected_vector_length); + EXPECT_DOUBLE_EQ(vector.second.y(), 0.0); + params.heading = M_PI_2; + vector = forwardSimulatedSegment(point, params); + EXPECT_DOUBLE_EQ(vector.first.x(), point.x); + EXPECT_DOUBLE_EQ(vector.first.y(), point.y); + EXPECT_NEAR(vector.second.x(), 0.0, 1e-9); + EXPECT_DOUBLE_EQ(vector.second.y(), expected_vector_length); + params.heading = M_PI_4; + vector = forwardSimulatedSegment(point, params); + EXPECT_DOUBLE_EQ(vector.first.x(), point.x); + EXPECT_DOUBLE_EQ(vector.first.y(), point.y); + EXPECT_DOUBLE_EQ(vector.second.x(), std::sqrt(0.5) * expected_vector_length); + EXPECT_DOUBLE_EQ(vector.second.y(), std::sqrt(0.5) * expected_vector_length); + params.heading = -M_PI_2; + vector = forwardSimulatedSegment(point, params); + EXPECT_DOUBLE_EQ(vector.first.x(), point.x); + EXPECT_DOUBLE_EQ(vector.first.y(), point.y); + EXPECT_NEAR(vector.second.x(), 0.0, 1e-9); + EXPECT_DOUBLE_EQ(vector.second.y(), -expected_vector_length); + }; + + // 0 velocity: whatever the duration the vector length is always = to extra_dist + params.velocity = 0.0; + + params.duration = 0.0; + params.extra_length = 0.0; + check_vector(params.extra_length); + + params.duration = 5.0; + params.extra_length = 2.0; + check_vector(params.extra_length); + + params.duration = -5.0; + params.extra_length = 3.5; + check_vector(params.extra_length); + + // set non-zero velocities + params.velocity = 1.0; + + params.duration = 1.0; + params.extra_length = 0.0; + check_vector(1.0 + params.extra_length); + + params.duration = 5.0; + params.extra_length = 2.0; + check_vector(5.0 + params.extra_length); + + params.duration = -5.0; + params.extra_length = 3.5; + check_vector(-5.0 + params.extra_length); +} + +const auto point_in_polygon = [](const auto x, const auto y, const auto & polygon) { + return std::find_if(polygon.outer().begin(), polygon.outer().end(), [=](const auto & pt) { + return pt.x() == x && pt.y() == y; + }) != polygon.outer().end(); +}; + +TEST(TestForwardProjection, generateFootprint) +{ + using obstacle_velocity_limiter::generateFootprint; + using obstacle_velocity_limiter::linestring_t; + using obstacle_velocity_limiter::segment_t; + + auto footprint = generateFootprint(linestring_t{{0.0, 0.0}, {1.0, 0.0}}, 1.0); + EXPECT_TRUE(point_in_polygon(0.0, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(0.0, -1.0, footprint)); + EXPECT_TRUE(point_in_polygon(1.0, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(1.0, -1.0, footprint)); + footprint = generateFootprint(segment_t{{0.0, 0.0}, {1.0, 0.0}}, 1.0); + EXPECT_TRUE(point_in_polygon(0.0, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(0.0, -1.0, footprint)); + EXPECT_TRUE(point_in_polygon(1.0, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(1.0, -1.0, footprint)); + + footprint = generateFootprint(linestring_t{{0.0, 0.0}, {0.0, -1.0}}, 0.5); + EXPECT_TRUE(point_in_polygon(0.5, 0.0, footprint)); + EXPECT_TRUE(point_in_polygon(0.5, -1.0, footprint)); + EXPECT_TRUE(point_in_polygon(-0.5, 0.0, footprint)); + EXPECT_TRUE(point_in_polygon(-0.5, -1.0, footprint)); + footprint = generateFootprint(segment_t{{0.0, 0.0}, {0.0, -1.0}}, 0.5); + EXPECT_TRUE(point_in_polygon(0.5, 0.0, footprint)); + EXPECT_TRUE(point_in_polygon(0.5, -1.0, footprint)); + EXPECT_TRUE(point_in_polygon(-0.5, 0.0, footprint)); + EXPECT_TRUE(point_in_polygon(-0.5, -1.0, footprint)); + + footprint = generateFootprint(linestring_t{{-2.5, 5.0}, {2.5, 0.0}}, std::sqrt(2)); + EXPECT_TRUE(point_in_polygon(3.5, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(1.5, -1.0, footprint)); + EXPECT_TRUE(point_in_polygon(-3.5, 4.0, footprint)); + EXPECT_TRUE(point_in_polygon(-1.5, 6.0, footprint)); + footprint = generateFootprint(segment_t{{-2.5, 5.0}, {2.5, 0.0}}, std::sqrt(2)); + EXPECT_TRUE(point_in_polygon(3.5, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(1.5, -1.0, footprint)); + EXPECT_TRUE(point_in_polygon(-3.5, 4.0, footprint)); + EXPECT_TRUE(point_in_polygon(-1.5, 6.0, footprint)); +} + +TEST(TestForwardProjection, generateFootprintMultiLinestrings) +{ + using obstacle_velocity_limiter::generateFootprint; + using obstacle_velocity_limiter::linestring_t; + using obstacle_velocity_limiter::multilinestring_t; + + auto footprint = generateFootprint( + multilinestring_t{ + linestring_t{{0.0, 0.0}, {0.0, 1.0}}, linestring_t{{0.0, 0.0}, {0.8, 0.8}}, + linestring_t{{0.0, 0.0}, {1.0, 0.0}}}, + 0.5); + std::cout << boost::geometry::wkt(footprint) << std::endl; + /* + EXPECT_TRUE(point_in_polygon(-0.5, 0.0, footprint)); + EXPECT_TRUE(point_in_polygon(-0.5, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(0.5, 1.0, footprint)); + EXPECT_TRUE(point_in_polygon(1.0, -1.0, footprint)); + */ +} diff --git a/planning/obstacle_velocity_limiter/test/test_obstacles.cpp b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp new file mode 100644 index 0000000000000..70a0e94491b19 --- /dev/null +++ b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp @@ -0,0 +1,77 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/obstacles.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include + +TEST(TestObstacles, ObstacleTreePoints) +{ + /* + using obstacle_velocity_limiter::point_t; + const std::vector points = {point_t(0, 0), point_t(2, 2), point_t(10, -5)}; + obstacle_velocity_limiter::ObstacleTree tree(points); + + obstacle_velocity_limiter::polygon_t query; + query.outer() = {{-1, -1}, {-1, 1}, {1, 1}, {1, -1}, {-1, -1}}; + auto result = tree.intersections(query); + ASSERT_EQ(result.size(), 1lu); + EXPECT_EQ(result[0].x(), 0); + EXPECT_EQ(result[0].y(), 0); + + query.outer() = {{-1, -1}, {-1, 1}, {10, 10}, {10, -1}, {-1, -1}}; + result = tree.intersections(query); + ASSERT_EQ(result.size(), 2lu); + EXPECT_EQ(result[0].x(), 0); + EXPECT_EQ(result[0].y(), 0); + EXPECT_EQ(result[1].x(), 2); + EXPECT_EQ(result[1].y(), 2); + */ +} + +TEST(TestObstacles, ObstacleTreeLines) +{ + /* + using obstacle_velocity_limiter::point_t; + using obstacle_velocity_limiter::linestring_t; + const obstacle_velocity_limiter::multilinestring_t lines = { + {point_t{-0.5, -0.5}, point_t{0.5,0.5}}, + {point_t{0, 0}, point_t{1,1}}, + {point_t(2, 2), point_t(-2, 2)}, + {point_t{-3,-3}, point_t{-1,-1}, point_t{5, -5}} + }; + obstacle_velocity_limiter::ObstacleTree tree(lines); + + obstacle_velocity_limiter::polygon_t query; + query.outer() = {{-1, -1}, {-1, 1}, {1, 1}, {1, -1}, {-1, -1}}; + auto result = tree.intersections(query); + EXPECT_EQ(result.size(), 4lu); + for(const auto & point : result) + std::cout << boost::geometry::wkt(point) << std::endl; + + query.outer() = {{-2, 10}, {-1, 10}, {-1, -10}, {-2, -10}, {-2, 10}}; + result = tree.intersections(query); + EXPECT_EQ(result.size(), 4lu); + for(const auto & point : result) + std::cout << boost::geometry::wkt(point) << std::endl; + + query.outer() = {{0.1, -0.1}, {1, -0.1}, {1, -1}, {0.1, -1}, {0.1, -0.1}}; + result = tree.intersections(query); + for(const auto & point : result) + std::cout << boost::geometry::wkt(point) << std::endl; + */ +} diff --git a/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp new file mode 100644 index 0000000000000..486dfe74eed73 --- /dev/null +++ b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp @@ -0,0 +1,63 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" +#include "obstacle_velocity_limiter/types.hpp" + +#include + +#include + +TEST(TestOccupancyGridUtils, extractObstacleLines) +{ + using obstacle_velocity_limiter::multipolygon_t; + using obstacle_velocity_limiter::polygon_t; + constexpr int8_t occupied_thr = 10; + nav_msgs::msg::OccupancyGrid occupancy_grid; + occupancy_grid.info.height = 5; + occupancy_grid.info.width = 5; + occupancy_grid.data = + std::vector(occupancy_grid.info.height * occupancy_grid.info.width); + occupancy_grid.info.resolution = 1.0; + + polygon_t full_mask; + full_mask.outer() = {{0.0, 0.0}, {0.0, 5.0}, {5.0, 5.0}, {5.0, 0.0}, {0.0, 0.0}}; + + constexpr auto extractObstacles = []( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const multipolygon_t & negative_masks, + const polygon_t & positive_mask, const double thr) { + obstacle_velocity_limiter::ObstacleMasks masks; + masks.negative_masks = negative_masks; + masks.positive_mask = positive_mask; + auto grid_map = obstacle_velocity_limiter::convertToGridMap(occupancy_grid); + obstacle_velocity_limiter::threshold(grid_map, thr); + obstacle_velocity_limiter::maskPolygons(grid_map, masks); + return obstacle_velocity_limiter::extractObstacles(grid_map, occupancy_grid); + }; + auto obstacles = extractObstacles(occupancy_grid, {}, full_mask, occupied_thr); + EXPECT_TRUE(obstacles.empty()); + + occupancy_grid.data.at(5) = 10; + obstacles = extractObstacles(occupancy_grid, {}, full_mask, occupied_thr); + EXPECT_EQ(obstacles.size(), 1ul); + + for (auto i = 1; i < 4; ++i) + for (auto j = 1; j < 4; ++j) occupancy_grid.data[j + i * occupancy_grid.info.width] = 10; + obstacles = extractObstacles(occupancy_grid, {}, full_mask, occupied_thr); + EXPECT_EQ(obstacles.size(), 1ul); + + obstacles = extractObstacles(occupancy_grid, {full_mask}, full_mask, occupied_thr); + EXPECT_EQ(obstacles.size(), 0ul); +} diff --git a/system/default_ad_api/document/motion.drawio.svg b/system/default_ad_api/document/motion.drawio.svg new file mode 100644 index 0000000000000..c6e2593ed0eb7 --- /dev/null +++ b/system/default_ad_api/document/motion.drawio.svg @@ -0,0 +1,275 @@ + + + + + + + + + + + + + +
+
+
+ Paused +
+
+
+ + Paused + + + + + + + + +
+
+
+ Resuming +
+
+
+ + Resuming + + + + + + + + + + +
+
+
+ Resumed +
+
+
+ + Resumed + + + + + + + + +
+
+
+ Pausing +
+
+
+ + Pausing + + + + + + + + +
+
+
+ Moving +
+
+
+ + Moving + + + + + + + +
+
+
+ stop = true +
+
+
+ + stop = true + + + + + + +
+
+
+ stop = false +
+
+
+ + stop = false + + + + + + + +
+
+
+ pause = true +
+
+
+ + pause = true + + + + + + +
+
+
+ pause = false +
+
+
+ + pause = false + + + + + + + + +
+
+
+ Starting +
+
+
+ + Starting + + + + + + +
+
+
+ accept +
+
+
+ + accept + + + + + + + +
+
+
+ cmd vel = 0 +
+
+
+ + cmd vel = 0 + + + + + + +
+
+
+ cmd vel > 0 +
+
+
+ + + cmd vel > 0 + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/system/default_ad_api/src/motion.cpp b/system/default_ad_api/src/motion.cpp index d3bb4a19c6e4a..d99f72f437685 100644 --- a/system/default_ad_api/src/motion.cpp +++ b/system/default_ad_api/src/motion.cpp @@ -25,7 +25,7 @@ MotionNode::MotionNode(const rclcpp::NodeOptions & options) { stop_check_duration_ = declare_parameter("stop_check_duration", 1.0); require_accept_start_ = declare_parameter("require_accept_start", false); - waiting_for_set_pause_ = false; + is_calling_set_pause_ = false; const auto adaptor = component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -37,20 +37,68 @@ MotionNode::MotionNode(const rclcpp::NodeOptions & options) rclcpp::Rate rate(10); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); - change_state(State::Moving, true); + state_ = State::Unknown; } -void MotionNode::change_state(const State state, const bool init) +void MotionNode::update_state() +{ + if (!is_paused_ || !is_start_requested_) { + return; + } + + const auto get_next_state = [this]() { + if (is_paused_.value()) { + if (!is_start_requested_.value()) { + return State::Paused; + } else { + return require_accept_start_ ? State::Starting : State::Resuming; + } + } else { + if (!vehicle_stop_checker_.isVehicleStopped(stop_check_duration_)) { + return State::Moving; + } else { + return is_start_requested_.value() ? State::Resumed : State::Pausing; + } + } + }; + const auto next_state = get_next_state(); + + // Once the state becomes pausing, it must become a state where is_paused is true + if (state_ == State::Pausing) { + switch (next_state) { + case State::Paused: + case State::Starting: + case State::Resuming: + break; + case State::Moving: + case State::Pausing: + case State::Resumed: + case State::Unknown: + return; + } + } + + // Prevents transition from starting to resuming + if (state_ == State::Resuming && next_state == State::Starting) { + return; + } + + change_state(next_state); +} + +void MotionNode::change_state(const State state) { using MotionState = autoware_ad_api::motion::State::Message; static const auto mapping = std::unordered_map( - {{State::Moving, MotionState::MOVING}, + {{State::Unknown, MotionState::UNKNOWN}, {State::Pausing, MotionState::STOPPED}, {State::Paused, MotionState::STOPPED}, - {State::Resuming, MotionState::STARTING}, - {State::Resumed, MotionState::STARTING}}); + {State::Starting, MotionState::STARTING}, + {State::Resuming, MotionState::MOVING}, + {State::Resumed, MotionState::MOVING}, + {State::Moving, MotionState::MOVING}}); - if (init || mapping.at(state_) != mapping.at(state)) { + if (mapping.at(state_) != mapping.at(state)) { MotionState msg; msg.stamp = now(); msg.state = mapping.at(state); @@ -59,90 +107,50 @@ void MotionNode::change_state(const State state, const bool init) state_ = state; } -void MotionNode::on_timer() +void MotionNode::change_pause(bool pause) { - if (state_ == State::Moving) { - if (vehicle_stop_checker_.isVehicleStopped(stop_check_duration_)) { - change_state(State::Pausing); - } + if (!is_calling_set_pause_ && cli_set_pause_->service_is_ready()) { + const auto req = std::make_shared(); + req->pause = pause; + is_calling_set_pause_ = true; + cli_set_pause_->async_send_request(req, [this](auto) { is_calling_set_pause_ = false; }); } +} - if (state_ == State::Resumed) { - if (!vehicle_stop_checker_.isVehicleStopped(stop_check_duration_)) { - change_state(State::Moving); - } - } +void MotionNode::on_timer() +{ + update_state(); if (state_ == State::Pausing) { - if (!waiting_for_set_pause_ && cli_set_pause_->service_is_ready()) { - const auto req = std::make_shared(); - req->pause = true; - waiting_for_set_pause_ = true; - cli_set_pause_->async_send_request(req, [this](auto) { waiting_for_set_pause_ = false; }); - } + return change_pause(true); } - if (state_ == State::Resuming) { - if (!waiting_for_set_pause_ && !require_accept_start_) { - const auto req = std::make_shared(); - req->pause = false; - waiting_for_set_pause_ = true; - cli_set_pause_->async_send_request(req, [this](auto) { waiting_for_set_pause_ = false; }); - } + return change_pause(false); } } void MotionNode::on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg) { - switch (state_) { - case State::Moving: - case State::Pausing: - case State::Resumed: - if (msg->data) { - change_state(State::Paused); - } - break; - case State::Paused: - case State::Resuming: - if (!msg->data) { - change_state(State::Resumed); - } - break; - } + is_paused_ = msg->data; } void MotionNode::on_is_start_requested( const control_interface::IsStartRequested::Message::ConstSharedPtr msg) { - if (msg->data) { - if (state_ == State::Paused) { - return change_state(State::Resuming); - } - } else { - if (state_ == State::Resuming) { - return change_state(State::Paused); - } - if (state_ == State::Resumed) { - return change_state(State::Pausing); - } - } + is_start_requested_ = msg->data; } void MotionNode::on_accept( const autoware_ad_api::motion::AcceptStart::Service::Request::SharedPtr, const autoware_ad_api::motion::AcceptStart::Service::Response::SharedPtr res) { - if (state_ != State::Resuming) { + if (state_ != State::Starting) { using AcceptStartResponse = autoware_ad_api::motion::AcceptStart::Service::Response; throw component_interface_utils::ServiceException( AcceptStartResponse::ERROR_NOT_STARTING, "The motion state is not starting"); } - - const auto inner_req = std::make_shared(); - inner_req->pause = false; - - const auto inner_res = cli_set_pause_->call(inner_req); - component_interface_utils::status::copy(inner_res, res); // NOLINT + change_state(State::Resuming); + res->status.success = true; } } // namespace default_ad_api diff --git a/system/default_ad_api/src/motion.hpp b/system/default_ad_api/src/motion.hpp index 7b7d1b65dc49e..4d929d80c9655 100644 --- a/system/default_ad_api/src/motion.hpp +++ b/system/default_ad_api/src/motion.hpp @@ -43,14 +43,18 @@ class MotionNode : public rclcpp::Node Sub sub_is_paused_; Sub sub_is_start_requested_; - enum class State { Pausing, Paused, Resuming, Resumed, Moving }; + enum class State { Unknown, Pausing, Paused, Starting, Resuming, Resumed, Moving }; State state_; + std::optional is_paused_; + std::optional is_start_requested_; double stop_check_duration_; bool require_accept_start_; - bool waiting_for_set_pause_; + bool is_calling_set_pause_; - void change_state(const State state, const bool init = false); + void update_state(); + void change_state(const State state); + void change_pause(bool pause); void on_timer(); void on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg); void on_is_start_requested( diff --git a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp index 24ed43500a1ee..3c8eba3072126 100644 --- a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp +++ b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp @@ -16,6 +16,7 @@ #include "gtest/gtest.h" #include "raw_vehicle_cmd_converter/accel_map.hpp" #include "raw_vehicle_cmd_converter/brake_map.hpp" +#include "raw_vehicle_cmd_converter/pid.hpp" #include "raw_vehicle_cmd_converter/steer_map.hpp" #include @@ -49,6 +50,7 @@ using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; +using raw_vehicle_cmd_converter::PIDController; using raw_vehicle_cmd_converter::SteerMap; double epsilon = 1e-4; // may throw PackageNotFoundError exception for invalid package @@ -70,6 +72,21 @@ bool loadSteerMapData(SteerMap & steer_map) return steer_map.readSteerMapFromCSV(map_path + "test_steer_map.csv"); } +PIDController createSteerPid( + double kp, double ki, double kd, double max_ret, double min_ret, double max_ret_p, + double min_ret_p, double max_ret_i, double min_ret_i, double max_ret_d, double min_ret_d, + double invalid_integration_decay) +{ + PIDController steer_pid; + steer_pid.setDecay(invalid_integration_decay); + steer_pid.setGains(kp, ki, kd); + steer_pid.setLimits( + max_ret, min_ret, max_ret_p, min_ret_p, max_ret_i, min_ret_i, max_ret_d, min_ret_d); + steer_pid.setInitialized(); + + return steer_pid; +} + TEST(ConverterTests, LoadExampleMap) { AccelMap accel_map; @@ -212,3 +229,69 @@ TEST(ConverterTests, SteerMapCalculation) // case for interpolation EXPECT_DOUBLE_EQ(calcSteer(5.0, 5.0), 5.0); } + +TEST(PIDTests, calculateFB) +{ + PIDController steer_pid; + + double dt = 0.1; + double vel = 5.0; + double target_value = 0.15; + double current_value; + std::vector pid_contributions(3, 0.0); + std::vector pid_errors(3, 0.0); + double fb_value; + + std::vector fb_values{8.0, 8.0, 8.0, 8.0, 8.0, 7.85, 6.4, 4.9, 3.4, 1.9}; + std::vector ret_p{8.0, 8.0, 8.0, 8.0, 8.0, 7.5, 6.0, 4.5, 3.0, 1.5}; + std::vector ret_i{0.225, 0.42, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5}; + std::vector ret_d{0.0, -0.15, -0.15, -0.15, -0.15, -0.15, -0.1, -0.1, -0.1, -0.1}; + + steer_pid = createSteerPid(150.0, 15.0, 1.0, 8.0, -8.0, 8.0, -8.0, 0.5, -0.5, 0.15, -0.15, 0.97); + current_value = 0; + for (int i = 0; i < 10; i++) { + fb_value = + steer_pid.calculateFB(target_value, dt, vel, current_value, pid_contributions, pid_errors); + EXPECT_NEAR(fb_value, fb_values.at(i), epsilon); + EXPECT_NEAR(pid_contributions.at(0), ret_p.at(i), epsilon); + EXPECT_NEAR(pid_contributions.at(1), ret_i.at(i), epsilon); + EXPECT_NEAR(pid_contributions.at(2), ret_d.at(i), epsilon); + + if (i < 5) { + current_value += 0.02; + } else { + current_value += 0.01; + } + } + + steer_pid.reset(); + current_value = 0; + for (int i = 0; i < 10; i++) { + fb_value = + steer_pid.calculateFB(-target_value, dt, vel, current_value, pid_contributions, pid_errors); + EXPECT_NEAR(fb_value, -fb_values.at(i), epsilon); + EXPECT_NEAR(pid_contributions.at(0), -ret_p.at(i), epsilon); + EXPECT_NEAR(pid_contributions.at(1), -ret_i.at(i), epsilon); + EXPECT_NEAR(pid_contributions.at(2), -ret_d.at(i), epsilon); + + if (i < 5) { + current_value -= 0.02; + } else { + current_value -= 0.01; + } + } + + // invalid_integration_decay + steer_pid.reset(); + current_value = 0; + vel = 5.0; + steer_pid.calculateFB(target_value, dt, vel, current_value, pid_contributions, pid_errors); + current_value = 0.02; + vel = 0.001; + fb_value = + steer_pid.calculateFB(target_value, dt, vel, current_value, pid_contributions, pid_errors); + EXPECT_NEAR(fb_value, 8.0, epsilon); + EXPECT_NEAR(pid_contributions.at(0), 8.0, epsilon); + EXPECT_NEAR(pid_contributions.at(1), 0.21825, epsilon); + EXPECT_NEAR(pid_contributions.at(2), -0.15, epsilon); +}