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

chore: sync upstream

.pre-commit-config.yaml

@@ -4,7 +4,7 @@ ci:
 
 repos:
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.2.0
+    rev: v4.3.0
     hooks:
       - id: check-json
       - id: check-merge-conflict
@@ -67,7 +67,7 @@ repos:
         args: [--line-length=100]
 
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v14.0.1
+    rev: v14.0.4-1
     hooks:
       - id: clang-format
         types_or: [c++, c, cuda]

common/autoware_ad_api_msgs/README.md

@@ -2,8 +2,7 @@
 
 ## ResponseStatus
 
-This message is a response status commonly used in the service type API. For details, see [the response status][docs-response-status].
-Each API can define its own status codes.
+This message is a response status commonly used in the service type API. Each API can define its own status codes.
 The status codes are primarily used to indicate the error cause, such as invalid parameter and timeout.
 If the API succeeds, set success to true, code to zero, and message to the empty string.
 Alternatively, codes and messages can be used for warnings or additional information.
@@ -17,9 +16,11 @@ The status code zero is reserved for success. The status code 50000 or over are
 
 ## InterfaceVersion
 
-This message is for the interface version of the set of AD APIs. For details, see [the interface feature][docs-interface].
+Considering the product life cycle, there may be multiple vehicles using different versions of the AD API due to changes in requirements or functional improvements. For example, one vehicle uses `v1` for stability and another vehicle uses `v2` to enable more advanced functionality.
+
+In that situation, the AD API users such as developers of a web service have to switch the application behavior based on the version that each vehicle uses.
+The version of AD API follows [Semantic Versioning][semver] in order to provide an intuitive understanding of the changes between versions.
 
 <!-- link -->
 
-[docs-response-status]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/#response-status
-[docs-interface]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/interface/
+[semver]: https://semver.org/

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

@@ -348,6 +348,63 @@ inline geometry_msgs::msg::Pose calcOffsetPose(
   tf2::toMsg(tf_pose * tf_offset, pose);
   return pose;
 }
+
+/**
+ * @brief Calculate a point by linear interpolation.
+ */
+template <class Point1, class Point2>
+geometry_msgs::msg::Point calcInterpolatedPoint(
+  const Point1 & src, const Point2 & dst, const double ratio)
+{
+  const auto src_point = getPoint(src);
+  const auto dst_point = getPoint(dst);
+
+  tf2::Vector3 src_vec;
+  src_vec.setX(src_point.x);
+  src_vec.setY(src_point.y);
+  src_vec.setZ(src_point.z);
+
+  tf2::Vector3 dst_vec;
+  dst_vec.setX(dst_point.x);
+  dst_vec.setY(dst_point.y);
+  dst_vec.setZ(dst_point.z);
+
+  // Get pose by linear interpolation
+  const auto & vec = tf2::lerp(src_vec, dst_vec, ratio);
+
+  geometry_msgs::msg::Point point;
+  point.x = vec.x();
+  point.y = vec.y();
+  point.z = vec.z();
+
+  return point;
+}
+
+/**
+ * @brief Calculate a pose by linear interpolation.
+ */
+template <class Pose1, class Pose2>
+geometry_msgs::msg::Pose calcInterpolatedPose(
+  const Pose1 & src_pose, const Pose2 & dst_pose, const double ratio)
+{
+  tf2::Transform src_tf, dst_tf;
+  tf2::fromMsg(getPose(src_pose), src_tf);
+  tf2::fromMsg(getPose(dst_pose), dst_tf);
+
+  // Get pose by linear interpolation
+  const auto & point = tf2::lerp(src_tf.getOrigin(), dst_tf.getOrigin(), ratio);
+
+  // Get quaternion by spherical linear interpolation
+  const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), ratio);
+
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = point.x();
+  pose.position.y = point.y();
+  pose.position.z = point.z();
+  pose.orientation = tf2::toMsg(quaternion);
+
+  return pose;
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp

@@ -256,7 +256,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t
  */
 template <class T>
 double calcSignedArcLength(
-  const T & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx)
+  const T & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx)
 {
   validateNonEmpty(points);
 
@@ -269,6 +269,29 @@ double calcSignedArcLength(
   return signed_length_on_traj - signed_length_src_offset;
 }
 
+/**
+ * @brief calcSignedArcLength from point to index with maximum distance and yaw threshold
+ */
+template <class T>
+boost::optional<double> calcSignedArcLength(
+  const T & points, const geometry_msgs::msg::Pose & src_pose, const size_t dst_idx,
+  const double max_dist = std::numeric_limits<double>::max(),
+  const double max_yaw = std::numeric_limits<double>::max())
+{
+  validateNonEmpty(points);
+
+  const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw);
+  if (!src_seg_idx) {
+    return boost::none;
+  }
+
+  const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_idx);
+  const double signed_length_src_offset =
+    calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position);
+
+  return signed_length_on_traj - signed_length_src_offset;
+}
+
 /**
  * @brief calcSignedArcLength from index to point
  */

common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp

@@ -805,3 +805,120 @@ TEST(geometry, calcOffsetPose)
     EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831);
   }
 }
+
+TEST(geometry, calcInterpolatedPoint)
+{
+  using tier4_autoware_utils::calcInterpolatedPoint;
+  using tier4_autoware_utils::createPoint;
+
+  {
+    const auto src_point = createPoint(0.0, 0.0, 0.0);
+    const auto dst_point = createPoint(3.0, 0.0, 0.0);
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.x, 3.0 * ratio);
+      EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+    }
+  }
+
+  // Same points are given
+  {
+    const auto src_point = createPoint(0.0, 0.0, 0.0);
+    const auto dst_point = createPoint(0.0, 0.0, 0.0);
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPoint(src_point, dst_point, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.z, 0.0);
+    }
+  }
+}
+
+TEST(geometry, calcInterpolatedPose)
+{
+  using tier4_autoware_utils::calcInterpolatedPose;
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::createQuaternion;
+  using tier4_autoware_utils::createQuaternionFromRPY;
+  using tier4_autoware_utils::deg2rad;
+
+  // Position Interpolation
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0);
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(3.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0);
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.position.x, 3.0 * ratio);
+      EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+    }
+  }
+
+  // Quaternion Interpolation
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(0.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90));
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
+
+      const auto result_quat = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(90 * ratio));
+      EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.x, result_quat.x);
+      EXPECT_DOUBLE_EQ(p_out.orientation.y, result_quat.y);
+      EXPECT_DOUBLE_EQ(p_out.orientation.z, result_quat.z);
+      EXPECT_DOUBLE_EQ(p_out.orientation.w, result_quat.w);
+    }
+  }
+
+  // Same points are given
+  {
+    geometry_msgs::msg::Pose src_pose;
+    src_pose.position = createPoint(0.0, 0.0, 0.0);
+    src_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
+
+    geometry_msgs::msg::Pose dst_pose;
+    dst_pose.position = createPoint(0.0, 0.0, 0.0);
+    dst_pose.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(0));
+
+    const double epsilon = 1e-3;
+    for (double ratio = 0.0; ratio < 1.0 + epsilon; ratio += 0.1) {
+      const auto p_out = calcInterpolatedPose(src_pose, dst_pose, ratio);
+
+      EXPECT_DOUBLE_EQ(p_out.position.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.0);
+      EXPECT_DOUBLE_EQ(p_out.orientation.w, 1.0);
+    }
+  }
+}

common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp

@@ -497,6 +497,48 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex)
   EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(4.3, 7.0, 0.0), 2), -2.3, epsilon);
 }
 
+TEST(trajectory, calcSignedArcLengthFromPoseToIndex_DistThreshold)
+{
+  using tier4_autoware_utils::calcSignedArcLength;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+
+  // Out of threshold
+  EXPECT_FALSE(calcSignedArcLength(traj.points, createPose(0.0, 0.6, 0.0, 0.0, 0.0, 0.0), 3, 0.5));
+
+  // On threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0,
+    epsilon);
+
+  // Within threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.4, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0,
+    epsilon);
+}
+
+TEST(trajectory, calcSignedArcLengthFromPoseToIndex_YawThreshold)
+{
+  using tier4_autoware_utils::calcSignedArcLength;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+  const auto max_d = std::numeric_limits<double>::max();
+
+  // Out of threshold
+  EXPECT_FALSE(
+    calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.1), 3, max_d, 1.0));
+
+  // On threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.0), 3, max_d, 1.0), 3.0,
+    epsilon);
+
+  // Within threshold
+  EXPECT_NEAR(
+    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.9), 3, max_d, 1.0), 3.0,
+    epsilon);
+}
+
 TEST(trajectory, calcSignedArcLengthFromIndexToPoint)
 {
   using tier4_autoware_utils::calcSignedArcLength;