Merge branch 'main' into 602-evaluate-performance-of-localization-pip…

…eline

common/interpolation/CMakeLists.txt

@@ -8,6 +8,7 @@ ament_auto_add_library(interpolation SHARED
   src/linear_interpolation.cpp
   src/spline_interpolation.cpp
   src/spline_interpolation_points_2d.cpp
+  src/zero_order_hold.cpp
 )
 
 if(BUILD_TESTING)

common/interpolation/include/interpolation/zero_order_hold.hpp

@@ -0,0 +1,29 @@
+// 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 INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+#define INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+
+#include "interpolation/interpolation_utils.hpp"
+
+#include <vector>
+
+namespace interpolation
+{
+std::vector<double> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys, const double overlap_threshold = 1e-3);
+}  // namespace interpolation
+
+#endif  // INTERPOLATION__ZERO_ORDER_HOLD_HPP_

common/interpolation/src/zero_order_hold.cpp

@@ -0,0 +1,52 @@
+// 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 "interpolation/zero_order_hold.hpp"
+
+#include <vector>
+
+namespace interpolation
+{
+std::vector<double> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys, const double overlap_threshold)
+{
+  // throw exception for invalid arguments
+  interpolation_utils::validateKeys(base_keys, query_keys);
+  interpolation_utils::validateKeysAndValues(base_keys, base_values);
+
+  std::vector<double> query_values;
+  size_t closest_segment_idx = 0;
+  for (size_t i = 0; i < 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)) {
+      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)) {
+          // find closest segment in base keys
+          closest_segment_idx = j;
+        }
+      }
+    }
+
+    query_values.push_back(base_values.at(closest_segment_idx));
+  }
+
+  return query_values;
+}
+
+}  // namespace interpolation

common/interpolation/test/src/test_zero_order_hold.cpp

@@ -0,0 +1,97 @@
+// 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 "interpolation/zero_order_hold.hpp"
+
+#include <gtest/gtest.h>
+
+#include <limits>
+#include <vector>
+
+constexpr double epsilon = 1e-6;
+
+TEST(zero_order_hold_interpolation, vector_interpolation)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 2.5, 3.5, 0.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<double> ans{0.0, 0.0, 1.5, 6.0};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as base_keys
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-1.2, 1.0, 2.0};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 2.5, 3.5, 0.0};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001};
+    const std::vector<double> ans = {0.0, 1.5, 2.5, 3.5, 3.5};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 2.5, 3.5, 0.0};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001};
+    const std::vector<double> ans = {0.0, 1.5, 2.5, 3.5, 0.0};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}

common/tier4_autoware_utils/docs/vehicle/vehicle.md → common/motion_utils/docs/vehicle/vehicle.md

@@ -142,7 +142,7 @@ public:
 
 ```c++
 
-bool SampleNode::sampleFunc1()
+bool SampleNode::sampleFunc()
 {
   ...
 

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

@@ -89,6 +89,45 @@ void validateNonSharpAngle(
   }
 }
 
+template <class T>
+bool isDrivingForward(const T points)
+{
+  if (points.size() < 2) {
+    return true;
+  }
+
+  // check the first point direction
+  const auto & first_point_pose = tier4_autoware_utils::getPose(points.at(0));
+  const auto & second_point_pose = tier4_autoware_utils::getPose(points.at(1));
+
+  const double first_point_yaw = tf2::getYaw(first_point_pose.orientation);
+  const double driving_direction_yaw =
+    tier4_autoware_utils::calcAzimuthAngle(first_point_pose.position, second_point_pose.position);
+  if (
+    std::abs(tier4_autoware_utils::normalizeRadian(first_point_yaw - driving_direction_yaw)) <
+    tier4_autoware_utils::pi / 2.0) {
+    return true;
+  }
+
+  return false;
+}
+
+template <class T>
+bool isDrivingForwardWithTwist(const T points_with_twist)
+{
+  if (points_with_twist.empty()) {
+    return true;
+  }
+  if (points_with_twist.size() == 1) {
+    if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
+      return true;
+    }
+    return false;
+  }
+
+  return isDrivingForward(points_with_twist);
+}
+
 template <class T>
 boost::optional<size_t> searchZeroVelocityIndex(
   const T & points_with_twist, const size_t src_idx, const size_t dst_idx)
@@ -805,10 +844,13 @@ inline boost::optional<size_t> insertTargetPoint(
   const auto overlap_with_back =
     tier4_autoware_utils::calcDistance2d(p_target, p_back) < overlap_threshold;
 
+  const bool is_driving_forward = isDrivingForward(points);
+
   geometry_msgs::msg::Pose target_pose;
   {
-    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_back);
-    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_back);
+    const auto p_base = is_driving_forward ? p_back : p_front;
+    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base);
+    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base);
 
     target_pose.position = p_target;
     target_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
@@ -817,17 +859,22 @@ inline boost::optional<size_t> insertTargetPoint(
   auto p_insert = points.at(seg_idx);
   tier4_autoware_utils::setPose(target_pose, p_insert);
 
-  geometry_msgs::msg::Pose front_pose;
+  geometry_msgs::msg::Pose base_pose;
   {
-    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_front, p_target);
-    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_target);
+    const auto p_base = is_driving_forward ? p_front : p_back;
+    const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target);
+    const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target);
 
-    front_pose.position = tier4_autoware_utils::getPoint(points.at(seg_idx));
-    front_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
+    base_pose.position = tier4_autoware_utils::getPoint(p_base);
+    base_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
   }
 
   if (!overlap_with_front && !overlap_with_back) {
-    tier4_autoware_utils::setPose(front_pose, points.at(seg_idx));
+    if (is_driving_forward) {
+      tier4_autoware_utils::setPose(base_pose, points.at(seg_idx));
+    } else {
+      tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1));
+    }
     points.insert(points.begin() + seg_idx + 1, p_insert);
     return seg_idx + 1;
   }
@@ -1012,36 +1059,6 @@ inline boost::optional<size_t> insertStopPoint(
 
   return stop_idx;
 }
-
-template <class T>
-inline bool isDrivingForward(const T points_with_twist)
-{
-  // if points size is smaller than 2
-  if (points_with_twist.empty()) {
-    return true;
-  }
-  if (points_with_twist.size() == 1) {
-    if (0.0 <= tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) {
-      return true;
-    }
-    return false;
-  }
-
-  // check the first point direction
-  const auto & first_point_pose = tier4_autoware_utils::getPose(points_with_twist.at(0));
-  const auto & second_point_pose = tier4_autoware_utils::getPose(points_with_twist.at(1));
-
-  const double first_point_yaw = tf2::getYaw(first_point_pose.orientation);
-  const double driving_direction_yaw =
-    tier4_autoware_utils::calcAzimuthAngle(first_point_pose.position, second_point_pose.position);
-  if (
-    std::abs(tier4_autoware_utils::normalizeRadian(first_point_yaw - driving_direction_yaw)) <
-    M_PI_2) {
-    return true;
-  }
-
-  return false;
-}
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

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

@@ -1691,6 +1691,64 @@ TEST(trajectory, insertTargetPoint)
   }
 }
 
+TEST(trajectory, insertTargetPoint_Reverse)
+{
+  using motion_utils::calcArcLength;
+  using motion_utils::findNearestSegmentIndex;
+  using motion_utils::insertTargetPoint;
+  using tier4_autoware_utils::calcDistance2d;
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::createQuaternionFromYaw;
+  using tier4_autoware_utils::deg2rad;
+  using tier4_autoware_utils::getPose;
+
+  constexpr double overlap_threshold = 1e-4;
+  auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+  for (size_t i = 0; i < traj.points.size(); ++i) {
+    traj.points.at(i).pose.orientation = createQuaternionFromYaw(tier4_autoware_utils::pi);
+  }
+  const auto total_length = calcArcLength(traj.points);
+
+  for (double x_start = 0.0; x_start < total_length; x_start += 1.0) {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(x_start + 1.1e-4, 0.0, 0.0);
+    const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
+    const auto insert_idx =
+      insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
+
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
+    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+
+    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
+      EXPECT_TRUE(
+        calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold);
+    }
+
+    {
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180));
+      EXPECT_EQ(p_insert.position.x, p_target.x);
+      EXPECT_EQ(p_insert.position.y, p_target.y);
+      EXPECT_EQ(p_insert.position.z, p_target.z);
+      EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon);
+    }
+
+    {
+      const auto p_base = getPose(traj_out.points.at(base_idx));
+      const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180));
+      EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+}
+
 TEST(trajectory, insertTargetPoint_OverlapThreshold)
 {
   using motion_utils::calcArcLength;

common/tier4_autoware_utils/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(Boost REQUIRED)
 
 ament_auto_add_library(tier4_autoware_utils SHARED
   src/tier4_autoware_utils.cpp
+  src/geometry/boost_polygon_utils.cpp
 )
 
 if(BUILD_TESTING)