feat(trajectory): add populate function

soblin · soblin · commit 42f5eb6af531 · 2025-03-28T12:11:45.000+09:00
Signed-off-by: Mamoru Sobue &lt;mamoru.sobue@tier4.jp&gt;
diff --git a/common/autoware_trajectory/CMakeLists.txt b/common/autoware_trajectory/CMakeLists.txt
@@ -28,9 +28,11 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(autoware_pyplot REQUIRED)
   find_package(range-v3 REQUIRED)
+  find_package(autoware_test_utils REQUIRED)
   ament_lint_auto_find_test_dependencies()
   find_package(pybind11 REQUIRED)
   include_directories(${autoware_pyplot_INCLUDE_DIRS})
+  include_directories(${autoware_test_utils_INCLUDE_DIRS})
   foreach(example_file ${example_files})
     get_filename_component(example_name ${example_file} NAME_WE)
     ament_auto_add_executable(${example_name}
@@ -41,6 +43,7 @@ if(BUILD_TESTING)
       ${autoware_pyplot_LIBRARIES}
       range-v3::range-v3
       pybind11::embed
+      ${autoware_test_utils_LIBRARIES}
     )
   endforeach()
 endif()
diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/populate.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/populate.hpp
@@ -0,0 +1,65 @@
+// Copyright 2025 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 AUTOWARE__TRAJECTORY__UTILS__POPULATE_HPP_
+#define AUTOWARE__TRAJECTORY__UTILS__POPULATE_HPP_
+
+#include "autoware/trajectory/detail/types.hpp"
+#include "autoware/trajectory/forward.hpp"
+
+#include <tl_expected/expected.hpp>
+
+#include <autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::trajectory
+{
+
+/**
+ * @brief if the input point size is less than 3, add 3rd point, otherwise return as is
+ * @param[in] inputs vector of point whose size is at least 2
+ * @return the vector of points whose size is at least 3, or error reason
+ * @note {x, y, z}/orientation are interpolated by Linear/SphericalLinear. other properties as
+ * interpolated by StairStep
+ */
+tl::expected<std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId>, std::string>
+populate3(const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> & inputs);
+
+/**
+ * @brief if the input point size is less than 4, add 4th point, otherwise return as is
+ * @param[in] inputs inputs
+ * @return optional the vector of points whose size is at least 4, so that it can be interpolated by
+ * CubicSpline, or error reason
+ * @note {x, y, z}/orientation are interpolated by Linear/SphericalLinear. other properties as
+ * interpolated by StairStep
+ */
+tl::expected<std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId>, std::string>
+populate4(const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> & inputs);
+
+/**
+ * @brief if the input point size is less than 5, add 5th point, otherwise return as is
+ * @param[in] inputs inputs
+ * @return the vector of points whose size is at least 5, so that it can be interpolated by
+ * AkimaSpline, or error reason
+ * @note {x, y, z}/orientation are interpolated by Cubic/SphericalLinear. other properties as
+ * interpolated by StairStep
+std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> populate5(
+  const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> & inputs,
+  const size_t minimum_number_of_points);
+ */
+
+}  // namespace autoware::trajectory
+#endif  // AUTOWARE__TRAJECTORY__UTILS__POPULATE_HPP_
diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml
@@ -16,6 +16,7 @@
 
   <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_utils_geometry</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_core</depend>
   <depend>rclcpp</depend>
@@ -27,6 +28,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
   <test_depend>autoware_pyplot</test_depend>
+  <test_depend>autoware_test_utils</test_depend>
   <test_depend>pybind11_vendor</test_depend>
   <test_depend>range-v3</test_depend>
 
diff --git a/common/autoware_trajectory/src/utils/populate.cpp b/common/autoware_trajectory/src/utils/populate.cpp
@@ -0,0 +1,122 @@
+// Copyright 2025 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 "autoware/trajectory/utils/populate.hpp"
+
+#include "autoware/trajectory/interpolator/linear.hpp"
+#include "autoware/trajectory/interpolator/spherical_linear.hpp"
+#include "autoware/trajectory/path_point_with_lane_id.hpp"
+#include "autoware_utils_geometry/geometry.hpp"
+
+#include <string>
+#include <vector>
+
+namespace autoware::trajectory
+{
+
+tl::expected<std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId>, std::string>
+populate3(const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> & inputs)
+{
+  if (inputs.size() >= 3) {
+    return inputs;
+  }
+  if (inputs.size() < 2) {
+    return tl::unexpected("cannot populate3() from #" + std::to_string(inputs.size()) + " points!");
+  }
+
+  // assert(inputs.size() == 2);
+
+  const auto & p1 = inputs.at(0).point.pose;
+  const auto & pos1 = p1.position;
+  const auto & p2 = inputs.at(1).point.pose;
+  const auto & pos2 = p2.position;
+  const auto l = autoware_utils_geometry::calc_distance3d(pos1, pos2);
+
+  const auto quat_result =
+    interpolator::SphericalLinear::Builder{}
+      .set_bases(std::vector<double>{0.0, l})
+      .set_values(std::vector<geometry_msgs::msg::Quaternion>({p1.orientation, p2.orientation}))
+      .build();
+
+  // LCOV_EXCL_START
+  if (!quat_result) {
+    // this never happens because two values are given
+    return tl::unexpected(std::string("failed to interpolate orientation"));
+  }
+  // LCOV_EXCL_END
+
+  const geometry_msgs::msg::Point mid_position = geometry_msgs::build<geometry_msgs::msg::Point>()
+                                                   .x((pos1.x + pos2.x) / 2.0)
+                                                   .y((pos1.y + pos2.y) / 2.0)
+                                                   .z((pos1.z + pos2.z) / 2.0);
+  const auto mid_quat = quat_result->compute(l / 2.0);
+
+  autoware_internal_planning_msgs::msg::PathPointWithLaneId point;
+  point.point.pose.position = mid_position;
+  point.point.pose.orientation = mid_quat;
+  point.point.longitudinal_velocity_mps = inputs.at(0).point.longitudinal_velocity_mps;
+  point.point.lateral_velocity_mps = inputs.at(0).point.lateral_velocity_mps;
+  point.point.heading_rate_rps = inputs.at(0).point.heading_rate_rps;
+  point.lane_ids = inputs.at(0).lane_ids;
+
+  return std::vector{inputs[0], point, inputs[1]};
+}
+
+tl::expected<std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId>, std::string>
+populate4(const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> & inputs)
+{
+  if (inputs.size() >= 4) {
+    return inputs;
+  }
+  if (inputs.size() < 2) {
+    return tl::unexpected("cannot populate4() from #" + std::to_string(inputs.size()) + " points!");
+  }
+
+  const auto try_inputs3 = populate3(inputs);
+  if (!try_inputs3) {
+    return tl::unexpected(try_inputs3.error());
+  }
+  const auto & inputs3 = inputs.size() == 2 ? try_inputs3.value() : inputs;
+
+  const auto input3_interpolation_result =
+    Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>::Builder{}
+      .set_xy_interpolator<interpolator::Linear>()
+      .set_z_interpolator<interpolator::Linear>()
+      .set_orientation_interpolator<interpolator::SphericalLinear>()
+      .build(inputs3);
+
+  // LCOV_EXCL_START
+  if (!input3_interpolation_result) {
+    // actually this block is impossible because 3 points are given, which is sufficient for Linear
+    // interpolation
+    return tl::unexpected(std::string("failed Linear interpolation in populate4()!"));
+  }
+  // LCOV_EXCL_END
+
+  const auto & interpolation = input3_interpolation_result.value();
+
+  const auto bases = interpolation.get_internal_bases();
+  // assert(bases.size() == 3);
+  const auto bases_diff =
+    std::vector<double>{(bases.at(1) - bases.at(0)), (bases.at(2) - bases.at(1))};
+  if (bases_diff.at(0) >= bases_diff.at(1)) {
+    const auto new_base = (bases.at(0) + bases.at(1)) / 2.0;
+    return std::vector{
+      inputs3.at(0), interpolation.compute(new_base), inputs3.at(1), inputs3.at(2)};
+  }
+  const auto new_base = (bases.at(1) + bases.at(2)) / 2.0;
+  return std::vector{inputs3.at(0), inputs3.at(1), interpolation.compute(new_base), inputs3.at(2)};
+}
+
+}  // namespace autoware::trajectory
diff --git a/common/autoware_trajectory/test/test_populate.cpp b/common/autoware_trajectory/test/test_populate.cpp
