WIP

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

common/autoware_trajectory/include/autoware/trajectory/utils/autoware_msgs.hpp

@@ -24,6 +24,7 @@
 #include <autoware_planning_msgs/msg/path.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -64,15 +65,18 @@ std::optional<Trajectory<autoware_planning_msgs::msg::PathPoint>> from_autoware_
 std::optional<Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>> from_autoware_msgs(
   const autoware_planning_msgs::msg::Trajectory & path, const bool use_akima = false);
 
+namespace detail
+{
+
 /**
  * @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);
+template <typename PointType>
+tl::expected<std::vector<PointType>, std::string> populate3(const std::vector<PointType> & inputs);
 
 /**
  * @brief if the input point size is less than 4, add 4th point, otherwise return as is
@@ -82,8 +86,8 @@ populate3(const std::vector<autoware_internal_planning_msgs::msg::PathPointWithL
  * @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);
+template <typename PointType>
+tl::expected<std::vector<PointType>, std::string> populate4(const std::vector<PointType> & inputs);
 
 /**
  * @brief if the input point size is less than 5, add 5th point, otherwise return as is
@@ -96,6 +100,7 @@ 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 detail
 
 }  // namespace autoware::trajectory
 #endif  // AUTOWARE__TRAJECTORY__UTILS__AUTOWARE_MSGS_HPP_

common/autoware_trajectory/src/utils/autoware_msgs.cpp

@@ -25,6 +25,56 @@
 namespace autoware::trajectory
 {
 
+template <typename T>
+const geometry_msgs::msg::Pose & get_geometry_msgs_pose([[maybe_unused]] const T & point)
+{
+  static_assert("No specialization given");
+}
+
+template <>
+const geometry_msgs::msg::Pose & get_geometry_msgs_pose(
+  const autoware_internal_planning_msgs::msg::PathPointWithLaneId & point)
+{
+  return point.point.pose;
+}
+
+template <>
+const geometry_msgs::msg::Pose & get_geometry_msgs_pose(
+  const autoware_planning_msgs::msg::PathPoint & point)
+{
+  return point.pose;
+}
+
+template <>
+const geometry_msgs::msg::Pose & get_geometry_msgs_pose(
+  const autoware_planning_msgs::msg::TrajectoryPoint & point)
+{
+  return point.pose;
+}
+
+template <typename PointType>
+tl::expected<std::vector<PointType>, std::string> populate3(const std::vector<PointType> & inputs)
+{
+  if (inputs.size() >= 3) {
+    return inputs;
+  }
+  if (inputs.size() < 2) {
+    return tl::unexpected("cannot populate3() from #" + std::to_string(inputs.size()) + " points!");
+  }
+
+  const auto & p1 = get_geometry_msgs_pose<PointType>(inputs.at(0));
+  const auto & pos1 = p1.position;
+  const auto & p2 = get_geometry_msgs_pose<PointType>(inputs.at(1));
+  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();
+}
+
 tl::expected<std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId>, std::string>
 populate3(const std::vector<autoware_internal_planning_msgs::msg::PathPointWithLaneId> & inputs)
 {

common/autoware_trajectory/test/test_autoware_msgs.cpp

@@ -17,7 +17,6 @@
 
 #include <gtest/gtest.h>
 
-#include <optional>
 #include <vector>
 
 using autoware_internal_planning_msgs::msg::PathPointWithLaneId;