Merge pull request autowarefoundation#342 from tier4/sync-upstream

chore: sync upstream

.github/workflows/github-release.yaml

@@ -27,7 +27,7 @@ jobs:
           fi
 
           echo "ref-name=$REF_NAME" >> $GITHUB_OUTPUT
-          echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT"
+          echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT
 
       - name: Check out repository
         uses: actions/checkout@v3

.github/workflows/pre-commit.yaml

@@ -20,11 +20,6 @@ jobs:
         with:
           ref: ${{ github.event.pull_request.head.ref }}
 
-      - name: Set git config
-        uses: autowarefoundation/autoware-github-actions/set-git-config@v1
-        with:
-          token: ${{ steps.generate-token.outputs.token }}
-
       - name: Run pre-commit
         uses: autowarefoundation/autoware-github-actions/pre-commit@v1
         with:

.pre-commit-config-optional.yaml

@@ -1,6 +1,6 @@
 repos:
   - repo: https://github.com/tcort/markdown-link-check
-    rev: v3.11.0
+    rev: v3.10.3
     hooks:
       - id: markdown-link-check
         args: [--config=.markdown-link-check.json]

common/motion_utils/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(Boost REQUIRED)
 
 ament_auto_add_library(motion_utils SHARED
   src/motion_utils.cpp
+  src/distance/distance.cpp
   src/marker/marker_helper.cpp
   src/resample/resample.cpp
   src/trajectory/interpolation.cpp

common/motion_utils/include/motion_utils/distance/distance.hpp

@@ -0,0 +1,34 @@
+// Copyright 2023 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 MOTION_UTILS__DISTANCE__DISTANCE_HPP_
+#define MOTION_UTILS__DISTANCE__DISTANCE_HPP_
+
+#include <boost/optional.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <tuple>
+#include <vector>
+
+namespace motion_utils
+{
+boost::optional<double> calcDecelDistWithJerkAndAccConstraints(
+  const double current_vel, const double target_vel, const double current_acc, const double acc_min,
+  const double jerk_acc, const double jerk_dec);
+
+}  // namespace motion_utils
+
+#endif  // MOTION_UTILS__DISTANCE__DISTANCE_HPP_

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

@@ -1245,6 +1245,42 @@ inline boost::optional<size_t> insertStopPoint(
   return stop_idx;
 }
 
+/**
+ * @brief Insert deceleration point from the source pose
+ * @param src_point source point
+ * @param distance_to_decel_point  distance to deceleration point from the src point
+ * @param velocity velocity of stop point
+ * @param points_with_twist output points of trajectory, path, ... (with velocity)
+ */
+template <class T>
+boost::optional<size_t> insertDecelPoint(
+  const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point,
+  const double velocity, T & points_with_twist)
+{
+  const auto decel_point =
+    calcLongitudinalOffsetPoint(points_with_twist, src_point, distance_to_decel_point);
+
+  if (!decel_point) {
+    return {};
+  }
+
+  const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.get());
+  const auto insert_idx = insertTargetPoint(seg_idx, decel_point.get(), points_with_twist);
+
+  if (!insert_idx) {
+    return {};
+  }
+
+  for (size_t i = insert_idx.get(); i < points_with_twist.size(); ++i) {
+    const auto & original_velocity =
+      tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(i));
+    tier4_autoware_utils::setLongitudinalVelocity(
+      std::min(original_velocity, velocity), points_with_twist.at(i));
+  }
+
+  return insert_idx;
+}
+
 /**
  * @brief Insert orientation to each point in points container (trajectory, path, ...)
  * @param points points of trajectory, path, ... (input / output)

common/motion_utils/src/distance/distance.cpp

@@ -0,0 +1,263 @@
+// Copyright 2023 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 "motion_utils/distance/distance.hpp"
+
+namespace motion_utils
+{
+namespace
+{
+bool validCheckDecelPlan(
+  const double v_end, const double a_end, const double v_target, const double a_target,
+  const double v_margin, const double a_margin)
+{
+  const double v_min = v_target - std::abs(v_margin);
+  const double v_max = v_target + std::abs(v_margin);
+  const double a_min = a_target - std::abs(a_margin);
+  const double a_max = a_target + std::abs(a_margin);
+
+  if (v_end < v_min || v_max < v_end) {
+    return false;
+  }
+  if (a_end < a_min || a_max < a_end) {
+    return false;
+  }
+
+  return true;
+}
+
+/**
+ * @brief update traveling distance, velocity and acceleration under constant jerk.
+ * @param (x) current traveling distance [m/s]
+ * @param (v) current velocity [m/s]
+ * @param (a) current acceleration [m/ss]
+ * @param (j) target jerk [m/sss]
+ * @param (t) time [s]
+ * @return updated traveling distance, velocity and acceleration
+ */
+std::tuple<double, double, double> update(
+  const double x, const double v, const double a, const double j, const double t)
+{
+  const double a_new = a + j * t;
+  const double v_new = v + a * t + 0.5 * j * t * t;
+  const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t;
+
+  return {x_new, v_new, a_new};
+}
+
+/**
+ * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID
+ * ACCELERATION PROFILE). this type of profile has ZERO JERK time.
+ *
+ * [ACCELERATION PROFILE]
+ *  a  ^
+ *     |
+ *  a0 *
+ *     |*
+ * ----+-*-------------------*------> t
+ *     |  *                 *
+ *     |   *               *
+ *     | a1 ***************
+ *     |
+ *
+ * [JERK PROFILE]
+ *  j  ^
+ *     |
+ *     |               ja ****
+ *     |                  *
+ * ----+----***************---------> t
+ *     |    *
+ *     |    *
+ *  jd ******
+ *     |
+ *
+ * @param (v0) current velocity [m/s]
+ * @param (a0) current acceleration [m/ss]
+ * @param (vt) target velocity [m/s]
+ * @param (am) minimum deceleration [m/ss]
+ * @param (ja) maximum jerk [m/sss]
+ * @param (jd) minimum jerk [m/sss]
+ * @param (t_min) duration of constant deceleration [s]
+ * @return moving distance until velocity is reached vt [m]
+ */
+boost::optional<double> calcDecelDistPlanType1(
+  const double v0, const double vt, const double a0, const double am, const double ja,
+  const double jd, const double t_min)
+{
+  constexpr double epsilon = 1e-3;
+
+  // negative jerk time
+  const double j1 = am < a0 ? jd : ja;
+  const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0;
+  const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1);
+
+  // zero jerk time
+  const double t2 = epsilon < t_min ? t_min : 0.0;
+  const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2);
+
+  // positive jerk time
+  const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0;
+  const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3);
+
+  const double a_target = 0.0;
+  const double v_margin = 0.3;  // [m/s]
+  const double a_margin = 0.1;  // [m/s^2]
+
+  if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) {
+    return {};
+  }
+
+  return x3;
+}
+
+/**
+ * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE
+ * ACCELERATION PROFILE), This type of profile do NOT have ZERO JERK time.
+ *
+ * [ACCELERATION PROFILE]
+ *  a  ^
+ *     |
+ *  a0 *
+ *     |*
+ * ----+-*-----*--------------------> t
+ *     |  *   *
+ *     |   * *
+ *     | a1 *
+ *     |
+ *
+ * [JERK PROFILE]
+ *  j  ^
+ *     |
+ *     | ja ****
+ *     |    *
+ * ----+----*-----------------------> t
+ *     |    *
+ *     |    *
+ *  jd ******
+ *     |
+ *
+ * @param (v0) current velocity [m/s]
+ * @param (vt) target velocity [m/s]
+ * @param (a0) current acceleration [m/ss]
+ * @param (am) minimum deceleration [m/ss]
+ * @param (ja) maximum jerk [m/sss]
+ * @param (jd) minimum jerk [m/sss]
+ * @return moving distance until velocity is reached vt [m]
+ */
+boost::optional<double> calcDecelDistPlanType2(
+  const double v0, const double vt, const double a0, const double ja, const double jd)
+{
+  constexpr double epsilon = 1e-3;
+
+  const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd));
+  const double a1 = -std::sqrt(a1_square);
+
+  // negative jerk time
+  const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0;
+  const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1);
+
+  // positive jerk time
+  const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0;
+  const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2);
+
+  const double a_target = 0.0;
+  const double v_margin = 0.3;
+  const double a_margin = 0.1;
+
+  if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) {
+    return {};
+  }
+
+  return x2;
+}
+
+/**
+ * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION
+ * PROFILE). This type of profile has only positive jerk time.
+ *
+ * [ACCELERATION PROFILE]
+ *  a  ^
+ *     |
+ * ----+----*-----------------------> t
+ *     |   *
+ *     |  *
+ *     | *
+ *     |*
+ *  a0 *
+ *     |
+ *
+ * [JERK PROFILE]
+ *  j  ^
+ *     |
+ *  ja ******
+ *     |    *
+ *     |    *
+ * ----+----*-----------------------> t
+ *     |
+ *
+ * @param (v0) current velocity [m/s]
+ * @param (vt) target velocity [m/s]
+ * @param (a0) current acceleration [m/ss]
+ * @param (ja) maximum jerk [m/sss]
+ * @return moving distance until velocity is reached vt [m]
+ */
+boost::optional<double> calcDecelDistPlanType3(
+  const double v0, const double vt, const double a0, const double ja)
+{
+  constexpr double epsilon = 1e-3;
+
+  // positive jerk time
+  const double t_acc = (0.0 - a0) / ja;
+  const double t1 = epsilon < t_acc ? t_acc : 0.0;
+  const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1);
+
+  const double a_target = 0.0;
+  const double v_margin = 0.3;
+  const double a_margin = 0.1;
+
+  if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) {
+    return {};
+  }
+
+  return x1;
+}
+}  // namespace
+
+boost::optional<double> calcDecelDistWithJerkAndAccConstraints(
+  const double current_vel, const double target_vel, const double current_acc, const double acc_min,
+  const double jerk_acc, const double jerk_dec)
+{
+  constexpr double epsilon = 1e-3;
+  const double t_dec =
+    acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc;
+  const double t_acc = (0.0 - acc_min) / jerk_acc;
+  const double t_min = (target_vel - current_vel - current_acc * t_dec -
+                        0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) /
+                       acc_min;
+
+  // check if it is possible to decelerate to the target velocity
+  // by simply bringing the current acceleration to zero.
+  const auto is_decel_needed =
+    0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel;
+
+  if (t_min > epsilon) {
+    return calcDecelDistPlanType1(
+      current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min);
+  } else if (is_decel_needed || current_acc > epsilon) {
+    return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec);
+  }
+
+  return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc);
+}
+}  // namespace motion_utils