Merge pull request #168 from tier4/sync-upstream

chore: sync upstream

.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
 

.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

.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 }}

.github/workflows/clang-tidy-pr-comments.yaml

@@ -9,42 +9,53 @@ 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
         uses: actions/checkout@v3
 
       - 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)"
           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 }}

.markdown-link-check.json

@@ -4,6 +4,9 @@
     {
       "pattern": "^http://localhost"
     },
+    {
+      "pattern": "^http://127\\.0\\.0\\.1"
+    },
     {
       "pattern": "^https://github.com/.*/discussions/new"
     }

.markdownlint.yaml

@@ -3,6 +3,8 @@ default: true
 MD013: false
 MD024:
   siblings_only: true
+MD029:
+  style: ordered
 MD033: false
 MD041: false
 MD046: false

common/interpolation/include/interpolation/interpolation_utils.hpp

@@ -15,6 +15,7 @@
 #ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_
 #define INTERPOLATION__INTERPOLATION_UTILS_HPP_
 
+#include <algorithm>
 #include <array>
 #include <stdexcept>
 #include <vector>
@@ -51,7 +52,7 @@ inline bool isNotDecreasing(const std::vector<double> & x)
   return true;
 }
 
-inline void validateKeys(
+inline std::vector<double> validateKeys(
   const std::vector<double> & base_keys, const std::vector<double> & 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 <class T>

common/interpolation/include/interpolation/zero_order_hold.hpp

@@ -27,20 +27,20 @@ std::vector<T> zero_order_hold(
   const std::vector<double> & 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<T> 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;
         }

common/interpolation/src/linear_interpolation.cpp

@@ -28,13 +28,13 @@ std::vector<double> lerp(
   const std::vector<double> & 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<double> 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;
     }

common/interpolation/src/spherical_linear_interpolation.cpp

@@ -34,13 +34,13 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
   const std::vector<double> & 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<geometry_msgs::msg::Quaternion> 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;
     }

common/interpolation/src/spline_interpolation.cpp

@@ -222,7 +222,7 @@ std::vector<double> SplineInterpolation::getSplineInterpolatedValues(
   const std::vector<double> & 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<double> SplineInterpolation::getSplineInterpolatedValues(
 
   std::vector<double> 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,15 +247,15 @@ std::vector<double> SplineInterpolation::getSplineInterpolatedDiffValues(
   const std::vector<double> & 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;
   const auto & c = multi_spline_coef_.c;
 
   std::vector<double> 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;
     }

common/interpolation/test/src/test_interpolation_utils.cpp

@@ -95,6 +95,23 @@ TEST(interpolation_utils, validateKeys)
 
   const std::vector<double> 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<double> 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<double> 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)

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1066,7 +1066,7 @@ inline boost::optional<size_t> 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<size_t> 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;

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<float64_t> & q, const std::vector<float64_t> & l,
     const std::vector<float64_t> & u);
 
+  // Setter functions for warm start
+  bool setWarmStart(
+    const std::vector<double> & primal_variables, const std::vector<double> & dual_variables);
+  bool setPrimalVariables(const std::vector<double> & primal_variables);
+  bool setDualVariables(const std::vector<double> & 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<int64_t>(m_latest_work_info.iter); }